search

google-deepmind / mujoco

Multi-Joint dynamics with Contact. A general purpose physics simulator.
https://mujoco.org
Apache License 2.0
7.94k stars 796 forks source link

Implementing a Velcro-sticking arrangement using the adhesion actuator. #1790

Closed AkashVardhan7 closed 2 months ago

AkashVardhan7 commented 3 months ago

Hello, I am trying to develop a simulation of 3 link shape changing robots using Mujoco. In the experiments the robots have Velcro patches on their arms which bind in a certain configuration only i.e. M-F and not vice versa. I have implemented this in my model using patches belonging to specific groups 1 and 2 as a proxy for the M and F parts of the Velcro and colored them B and G. However since these patches are children of the arms, when I try to declare the adhesion actuator later on in my code I am having to use the parent body which are the arms for the adhesion actuator. I try to manually enforce this constraint by implementing an adhesion function which assigns an adhesion force to a pair of bodies coming in contact if they belong to groups 1 and 2 respectively, however since the whole robot arm is declared as the adhesion body it causes the arms to stick in unnatural ways. I am attaching my simulation file and the video I generated when implementing this algorithm.

https://github.com/google-deepmind/mujoco/assets/48886152/6c532332-63a6-4dfb-bcf2-e47f3490fad9 Here is my code.. Couldn't attach the file hence pasting it out.

import mujoco as mj from mujoco.glfw import glfw from time import sleep import numpy as np import random import matplotlib.pyplot as plt

Base XML configuration for a single robot with planes on the arms

modified_base_robot_xml = '''

<body name="{robot_name}_central_link" pos="{x_pos} {y_pos} 0" quat="{quat_w} {quat_x} {quat_y} {quat_z}">
    <joint name="{robot_name}_free_joint" type="free"/>
    <geom type="box" size="0.04 0.01 0.01" mass="0.1" rgba="1 0 0 1"/>
    <!-- Arm 1 -->
    <body name="{robot_name}_arm_1" pos="-0.07 0 0.0">
        <joint name="{robot_name}_hinge1" type="hinge" axis="0 1 0" pos="0.03 0 0.0" range="0 1.5708" damping="0.25" frictionloss="0.1" stiffness="0.25"/>
        <geom type="box" size="0.03 0.01 0.0025" mass="0.05" rgba="0 0 0 1" friction="1.0" margin="0.0001"/>
        <!-- Blue and Green Pads on one Face of Arm 1 -->
        <geom name="{robot_name}_pad1_arm1" type="box" size="0.02 0.002 0.00001" pos="0.0 0.004 0.0025" rgba="0.0 0.0 0.5 1.0" group="1" friction="1.0" margin="0.0001"/>
        <geom name="{robot_name}_pad2_arm1" type="box" size="0.02 0.002 0.00001" pos="0.0 -0.004 0.0025" rgba="0.2 0.8 0.2 1.0" group="2" friction="1.0" margin="0.0001"/>
        <!-- Blue and Green Pads on other Face of Arm 1 -->
        <geom name="{robot_name}_pad3_arm1" type="box" size="0.02 0.002 0.00001" pos="0.0 0.004 -0.0025" rgba="0.2 0.8 0.2 1.0" group="2" friction="1.0" margin="0.0001"/>
        <geom name="{robot_name}_pad4_arm1" type="box" size="0.02 0.002 0.00001" pos="0.0 -0.004 -0.0025" rgba="0.0 0.0 0.5 1.0" group="1" friction="1.0" margin="0.0001"/>
    </body>

     <!-- Arm 2 -->
    <body name="{robot_name}_arm_2" pos="0.07 0 0.0">
        <joint name="{robot_name}_hinge2" type="hinge" axis="0 -1 0" pos="-0.03 0 0.0" range="0 1.5708" damping="0.25" frictionloss="0.1" stiffness="0.25"/>
        <geom type="box" size="0.03 0.01 0.0025" mass="0.05" rgba="0 0 0 1" friction="1.0" margin="0.0001"/>
        <!-- Blue and Green Pads on one Face of Arm 2 -->
        <geom name="{robot_name}_pad1_arm2" type="box" size="0.02 0.002 0.00001" pos="0.0 0.004 0.0025" rgba="0.0 0.0 0.5 1.0" group="1" friction="1.0" margin="0.0001"/>
        <geom name="{robot_name}_pad2_arm2" type="box" size="0.02 0.002 0.00001" pos="0.0 -0.004 0.0025" rgba="0.2 0.8 0.2 1.0" group="2" friction="1.0" margin="0.0001"/>
        <!-- Blue and Green Pads on other Face of Arm 2 -->
        <geom name="{robot_name}_pad3_arm2" type="box" size="0.02 0.002 0.00001" pos="0.0 0.004 -0.0025" rgba="0.2 0.8 0.2 1.0" group="2" friction="1.0" margin="0.0001"/>
        <geom name="{robot_name}_pad4_arm2" type="box" size="0.02 0.002 0.00001" pos="0.0 -0.004 -0.0025" rgba="0.0 0.0 0.5 1.0" group="1" friction="1.0" margin="0.0001"/>
    </body>
</body>

'''

simend = 600 # Simulation time (15 seconds) relaxation_time = 1 # Relaxation time (2 seconds) print_camera_config = 0 # Set to 1 to print camera config

Initialize simulation parameters

num_robots = 10 x_range = (-0.15, 0.15) y_range = (-0.15, 0.15) robot_positions = [] gain = 8 ctrlrange = [0, 1]

For callback functions

button_left = False button_middle = False button_right = False lastx = 0 lasty = 0

def init_controller(model, data):

Initialize the controller here. This function is called once, at the beginning

pass

def apply_adhesion(model, data, gain, ctrlrange): print(f"data.xfrc_applied.shape: {data.xfrc_applied.shape}")

# Iterate over all contacts
for contact in data.contact:
    geom1 = contact.geom1
    geom2 = contact.geom2

    # Check if both geometries are part of the adhesion groups
    if model.geom_group[geom1] in (1, 2) and model.geom_group[geom2] in (1, 2):
        distance = np.linalg.norm(data.geom_xpos[geom1] - data.geom_xpos[geom2])
        print(f"distance between the pads is {distance}")

        if distance < 0.001:  # Threshold distance for adhesion
            print(f"Applying adhesion force between geom {geom1} and geom {geom2}")

            # Calculate the adhesion force based on the distance and gain
            force_magnitude = gain * (ctrlrange[1] - ctrlrange[0])
            force_direction = (data.geom_xpos[geom1] - data.geom_xpos[geom2]) / distance
            force = force_magnitude * force_direction
            print(f"Force: {force}")

            if geom1 < data.xfrc_applied.shape[0] and geom2 < data.xfrc_applied.shape[0]:
                data.xfrc_applied[geom1, :3] += force[:3]
                data.xfrc_applied[geom1, 3:] += force[3:]
                data.xfrc_applied[geom2, :3] -= force[:3]
                data.xfrc_applied[geom2, 3:] -= force[3:]
            else:
                print(f"Index {geom1} or {geom2} out of bounds for data.xfrc_applied with shape {data.xfrc_applied.shape}")

def controller(model, data):

Calculate the desired position for the arms based on the current time

# Allow some relaxation time before starting the controller

if data.time < relaxation_time:
    return 

cycle_time = 1 # Total cycle time in seconds
t = data.time % cycle_time  # Current time within the cycle
angle = (1.0*np.pi / 2) * np.sin(2 * np.pi * t / cycle_time)

# Set the desired position for all robots
for i in range(len(data.ctrl)//2):
    #print("i is",i)
    #sleep(1)
    data.ctrl[2*i] = angle  # Hinge joint for Arm 1
    data.ctrl[2*i + 1] = angle  # Hinge joint for Arm 2

#apply_adhesion(model,data)

def keyboard(window, key, scancode, act, mods): if act == glfw.PRESS and key == glfw.KEY_BACKSPACE: mj.mj_resetData(model, data) mj.mj_forward(model, data)

def keyboard(window, key, scancode, act, mods): if act == glfw.PRESS and key == glfw.KEY_BACKSPACE: mj.mj_resetData(model, data) mj.mj_forward(model, data)

Function to generate a random quaternion with a fixed 90 degrees rotation about the x-axis

def euler_to_quaternion(roll, pitch, yaw): """ Convert Euler angles to quaternion. Roll (x-axis rotation), Pitch (y-axis rotation), Yaw (z-axis rotation) """ qx = np.sin(roll/2) np.cos(pitch/2) np.cos(yaw/2) - np.cos(roll/2) np.sin(pitch/2) np.sin(yaw/2) qy = np.cos(roll/2) np.sin(pitch/2) np.cos(yaw/2) + np.sin(roll/2) np.cos(pitch/2) np.sin(yaw/2) qz = np.cos(roll/2) np.cos(pitch/2) np.sin(yaw/2) - np.sin(roll/2) np.sin(pitch/2) np.cos(yaw/2) qw = np.cos(roll/2) np.cos(pitch/2) np.cos(yaw/2) + np.sin(roll/2) np.sin(pitch/2) np.sin(yaw/2) return qw, qx, qy, qz

def random_quaternion(): """ Generate a random quaternion with a fixed 90 degrees rotation about the x-axis and random rotations about the y and z axes. """ roll = np.pi / 2 # 90 degrees about the x-axis pitch = random.uniform(0, 2 np.pi) # Random angle about the y-axis yaw = random.uniform(0, 2 np.pi) # Random angle about the z-axis

# Convert Euler angles to quaternion
return euler_to_quaternion(roll, pitch, yaw)

Function to initialize multiple robots in the simulation

def initialize_robots(num_robots, x_range, y_range): robots = [] for i in range(num_robots): x_pos = random.uniform(x_range) y_pos = random.uniform(y_range)

quat_w, quat_x, quat_y, quat_z = random_quaternion()

    quat = random_quaternion()
    robot_name = f"robot_{i}"
    #robot_xml = modified_base_robot_xml.format(robot_name=robot_name, x_pos=x_pos, y_pos=y_pos, 
                                               #quat_w=quat_w, quat_x=quat_x, quat_y=quat_y, quat_z=quat_z)
    robot_xml = modified_base_robot_xml.format(robot_name=robot_name, x_pos=x_pos, y_pos=y_pos, 
                                               quat_w=quat[0], quat_x=quat[1], quat_y=quat[2], quat_z=quat[3])
    robots.append(robot_xml)
return ''.join(robots)

Function to save robot positions and orientations

def save_robot_positions(data): global robot_positions pos = data.qpos[:3] quat = data.qpos[3:7] robot_positions.append((pos.copy(), quat.copy()))

Function to plot robot positions and orientations

def plot_robot_positions(): fig = plt.figure() ax = fig.add_subplot(111, projection='3d') for pos, quat in robot_positions: ax.scatter(pos[0], pos[1], pos[2])

ax.quiver(pos[0], pos[1], pos[2], quat[1], quat[2], quat[3], length=0.1)

plt.show()

def mouse_button(window, button, act, mods): global button_left, button_middle, button_right button_left = (glfw.get_mouse_button(window, glfw.MOUSE_BUTTON_LEFT) == glfw.PRESS) button_middle = (glfw.get_mouse_button(window, glfw.MOUSE_BUTTON_MIDDLE) == glfw.PRESS) button_right = (glfw.get_mouse_button(window, glfw.MOUSE_BUTTON_RIGHT) == glfw.PRESS) glfw.get_cursor_pos(window)

def mouse_move(window, xpos, ypos): global lastx, lasty, button_left, button_middle, button_right dx = xpos - lastx dy = ypos - lasty lastx = xpos lasty = ypos

if not button_left and not button_middle and not button_right:
    return

width, height = glfw.get_window_size(window)
mod_shift = (glfw.get_key(window, glfw.KEY_LEFT_SHIFT) == glfw.PRESS or
             glfw.get_key(window, glfw.KEY_RIGHT_SHIFT) == glfw.PRESS)

if button_right:
    action = mj.mjtMouse.mjMOUSE_MOVE_H if mod_shift else mj.mjtMouse.mjMOUSE_MOVE_V
elif button_left:
    action = mj.mjtMouse.mjMOUSE_ROTATE_H if mod_shift else mj.mjtMouse.mjMOUSE_ROTATE_V
else:
    action = mj.mjtMouse.mjMOUSE_ZOOM

mj.mjv_moveCamera(model, action, dx / height, dy / height, scene, cam)

def scroll(window, xoffset, yoffset): mj.mjv_moveCamera(model, mj.mjtMouse.mjMOUSE_ZOOM, 0.0, -0.05 * yoffset, scene, cam)

Initialize MuJoCo model

robots_xml = initialize_robots(num_robots, x_range, y_range) combined_xml = f'''

'''

with open('multi_robot.xml', 'w') as file: file.write(combined_xml)

Load the MuJoCo model from the XML string

model = mj.MjModel.from_xml_string(combined_xml) data = mj.MjData(model) cam = mj.MjvCamera() # Abstract camera opt = mj.MjvOption() # Visualization options

Initialize visualization

glfw.init() window = glfw.create_window(1200, 900, "MuJoCo Simulation", None, None) glfw.make_context_current(window) glfw.swap_interval(1)

Initialize visualization data structures

scene = mj.MjvScene(model, maxgeom=10000) context = mj.MjrContext(model, mj.mjtFontScale.mjFONTSCALE_150)

Install GLFW mouse and keyboard callbacks

glfw.set_key_callback(window, keyboard) glfw.set_cursor_pos_callback(window, mouse_move) glfw.set_mouse_button_callback(window, mouse_button) glfw.set_scroll_callback(window, scroll)

Initialize the controller

init_controller(model, data)

Set the controller

mj.set_mjcb_control(controller) apply_adhesion(model, data, gain, ctrlrange)

Simulation loop

while not glfw.window_should_close(window): time_prev = data.time

while data.time - time_prev < 0.00002:
    mj.mj_step(model, data)

if data.time >= simend:
    break

# Save robot positions at each step
save_robot_positions(data)

# Get framebuffer viewport
viewport_width, viewport_height = glfw.get_framebuffer_size(window)
viewport = mj.MjrRect(0, 0, viewport_width, viewport_height)

# Print camera configuration (help to initialize the view)
if print_camera_config == 1:
    print('cam.azimuth =', cam.azimuth, ';', 'cam.elevation =', cam.elevation, ';', 'cam.distance =', cam.distance)
    print('cam.lookat = np.array([', cam.lookat[0], ',', cam.lookat[1], ',', cam.lookat[2], '])')

# Update scene and render
mj.mjv_updateScene(model, data, opt, None, cam, mj.mjtCatBit.mjCAT_ALL, scene)
mj.mjr_render(viewport, scene, context)

# Swap OpenGL buffers (blocking call due to v-sync)
glfw.swap_buffers(window)

# Process pending GUI events, call GLFW callbacks
glfw.poll_events()

Plot the robot positions after the simulation ends

plot_robot_positions()

Terminate GLFW

glfw.terminate()

AkashVardhan7 commented 3 months ago

This didn't get pasted properly....

 modified_base_robot_xml = '''
    <!-- Central Link -->
    <body name="{robot_name}_central_link" pos="{x_pos} {y_pos} 0" quat="{quat_w} {quat_x} {quat_y} {quat_z}">
        <joint name="{robot_name}_free_joint" type="free"/>
        <geom type="box" size="0.04 0.01 0.01" mass="0.1" rgba="1 0 0 1"/>
        <!-- Arm 1 -->
        <body name="{robot_name}_arm_1" pos="-0.07 0 0.0">
            <joint name="{robot_name}_hinge1" type="hinge" axis="0 1 0" pos="0.03 0 0.0" range="0 1.5708" damping="0.25" frictionloss="0.1" stiffness="0.25"/>
            <geom type="box" size="0.03 0.01 0.0025" mass="0.05" rgba="0 0 0 1" friction="1.0" margin="0.0001"/>
            <!-- Blue and Green Pads on one Face of Arm 1 -->
            <geom name="{robot_name}_pad1_arm1" type="box" size="0.02 0.002 0.00001" pos="0.0 0.004 0.0025" rgba="0.0 0.0 0.5 1.0" group="1" friction="1.0" margin="0.0001"/>
            <geom name="{robot_name}_pad2_arm1" type="box" size="0.02 0.002 0.00001" pos="0.0 -0.004 0.0025" rgba="0.2 0.8 0.2 1.0" group="2" friction="1.0" margin="0.0001"/>
            <!-- Blue and Green Pads on other Face of Arm 1 -->
            <geom name="{robot_name}_pad3_arm1" type="box" size="0.02 0.002 0.00001" pos="0.0 0.004 -0.0025" rgba="0.2 0.8 0.2 1.0" group="2" friction="1.0" margin="0.0001"/>
            <geom name="{robot_name}_pad4_arm1" type="box" size="0.02 0.002 0.00001" pos="0.0 -0.004 -0.0025" rgba="0.0 0.0 0.5 1.0" group="1" friction="1.0" margin="0.0001"/>
        </body>

         <!-- Arm 2 -->
        <body name="{robot_name}_arm_2" pos="0.07 0 0.0">
            <joint name="{robot_name}_hinge2" type="hinge" axis="0 -1 0" pos="-0.03 0 0.0" range="0 1.5708" damping="0.25" frictionloss="0.1" stiffness="0.25"/>
            <geom type="box" size="0.03 0.01 0.0025" mass="0.05" rgba="0 0 0 1" friction="1.0" margin="0.0001"/>
            <!-- Blue and Green Pads on one Face of Arm 2 -->
            <geom name="{robot_name}_pad1_arm2" type="box" size="0.02 0.002 0.00001" pos="0.0 0.004 0.0025" rgba="0.0 0.0 0.5 1.0" group="1" friction="1.0" margin="0.0001"/>
            <geom name="{robot_name}_pad2_arm2" type="box" size="0.02 0.002 0.00001" pos="0.0 -0.004 0.0025" rgba="0.2 0.8 0.2 1.0" group="2" friction="1.0" margin="0.0001"/>
            <!-- Blue and Green Pads on other Face of Arm 2 -->
            <geom name="{robot_name}_pad3_arm2" type="box" size="0.02 0.002 0.00001" pos="0.0 0.004 -0.0025" rgba="0.2 0.8 0.2 1.0" group="2" friction="1.0" margin="0.0001"/>
            <geom name="{robot_name}_pad4_arm2" type="box" size="0.02 0.002 0.00001" pos="0.0 -0.004 -0.0025" rgba="0.0 0.0 0.5 1.0" group="1" friction="1.0" margin="0.0001"/>
        </body>
    </body>
'''
AkashVardhan7 commented 3 months ago

image

yuvaltassa commented 3 months ago

I didn't quite follow the question, and the video was not helpful, but I think there is something you might be missing. Yes, adhesion applies to all geoms in a body, but you can wrap some subset with a static body (no joints).

So instead of

<body>
  <geom name="1"/>
  <geom name="2"/>
</body>

where I only want geom 2 to be adhesive but I get unwanted adhesion from geom 1, I can do

<body>
  <geom name="1"/>
  <body name="static_child">
    <geom name="2"/>
  </body>
</body>

and connect the adhesion actuator to the static_child body.

AkashVardhan7 commented 3 months ago

Hi, thank you for your reply. I realize my question could have been clearer and more precise. I want to implement adhesion between 2 specific groups of objects. These objects will be children to the robot arms (in my case) and I assigned them groups 1 and 2. I can follow your advice and make them static bodies and then connect the adhesion actuator to them in part where I generate the combined xml's for the robots. How would I be able to ensure that the adhesion between them is directional, which is to say adhesion happens only when a static body belonging to group 1 comes in contact with a static body belonging to group 2? Currently I have a function in my code which does this, where I check for the group identity of a pair of colliding bodies and assign them a force based on the specified gain and ctrl-range. I am not sure if this is the right way.

yuvaltassa commented 2 months ago

In the current design, if body 1 has an adhesion actuator then if body 2 can contact body 1, adhesion force can be generated between them. So given the current design you need a workaround similar to what you are doing.

Basically what you describe is a feature request. I doubt it will happen since this is a pretty rare use case.

However, it is very very easy to just modify the related code and rebuild from source, if you don't mind a bit of hacking. See this block here?

// irrelevant contact, continue
if (b1 != id && b2 != id) {
  continue;
}

just add similar thing just after like 

// geom ids
int g1 = con->geom[0];
int g2 = con->geom[1];

// contact not between "velcro" geoms, continue
if ((m->geom_group[g1] != 1 && m->geom_group[g2] != 2) || 
     m->geom_group[g2] != 1 && m->geom_group[g1] != 2)) {
  continue;
}

(or whatever other condition you want)

AkashVardhan7 commented 2 months ago

Thanks for your help on this. I can try out the solution you suggested and post in the forum to let you guys know how it went. Btw, I work in the same lab as Tingnan Zhang who is a part of your team .