Gripper of Robotiq 2f85 with UR5e fail to grasp an object. #205
Description
I am working on a project of using UR5e with robotiq 2f85 gripper to pick up a cube off the table. Unfortunately, the gripper failed to grasp the cube when try to lift up the cube.
Screen.Recording.2025-09-22.094454.mp4
Here is the xml file and minimal code to reproduce:
<!-- scene2.xml - Enhanced scene with floor and lighting -->
<mujoco model="ur5e scene">
<include file="ur5e.xml" />
<statistic center="0.3 0 0.3" extent="0.8" meansize="0.08" />
<default>
<default class="soft">
<geom type="mesh" contype="1" conaffinity="1" friction="1 0.1 0.1" />
</default>
</default>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0" />
<global azimuth="120" elevation="-20" />
</visual>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0" />
<global azimuth="120" elevation="-20" />
</visual>
<asset>
<texture name="grid" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" width="512"
height="512" mark="cross" markrgb=".8 .8 .8" />
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" />
</asset>
<worldbody>
<light pos="0 0 1.5" directional="true" />
<geom name="floor" size="1 1 0.01" type="plane" material="grid" />
</worldbody>
<sensor>
<!-- Additional sensors can be added here if needed -->
</sensor>
</mujoco>
<!-- ur5e.xml -->
<mujoco model="ur5e">
<compiler angle="radian" meshdir="assets" autolimits="true" />
<option integrator="implicitfast" />
<default>
<default class="ur5e">
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" group="2" />
</default>
<default class="collision">
<geom type="capsule" group="3" />
<default class="eef_collision">
<geom type="cylinder" />
</default>
</default>
<joint axis="0 1 0" range="-6.28319 6.28319" />
<default class="joint_limited">
<joint range="-3.1415 3.1415" />
</default>
<default class="position">
<position ctrlrange="-6.2831 6.2831" kp="2000" kv="100" forcerange="-150 150" />
<default class="position_limited">
<position ctrlrange="-3.1415 3.1415" />
</default>
<default class="position_small">
<position kp="500" kv="25" forcerange="-28 28" />
</default>
</default>
</default>
<default class="g2f85">
<mesh scale="0.001 0.001 0.001" />
<general biastype="affine" />
<joint axis="1 0 0" />
<default class="driver">
<joint range="0 0.8" armature="0.5" damping="0.1" solimplimit="0.95 0.99 0.001"
solreflimit="0.005 1" />
</default>
<default class="follower">
<joint range="-0.872664 0.872664" armature="0.5" pos="0 -0.018 0.0065"
solimplimit="0.95 0.99 0.001" solreflimit="0.005 1" />
</default>
<default class="spring_link">
<joint range="-0.29670597283 0.8" armature="0.001" stiffness="0.05" springref="2.62"
damping="0.00125" />
</default>
<default class="coupler">
<joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001"
solreflimit="0.005 1" />
</default>
<default class="2f85_visual">
<geom type="mesh" contype="0" conaffinity="0" group="2" />
</default>
<default class="2f85_collision">
<geom type="mesh" group="3" />
<default class="pad_box1">
<geom mass="0" type="box" pos="0 -0.0026 0.028125" size="0.011 0.004 0.009375"
friction="0.7"
solimp="0.95 0.99 0.001" solref="0.004 1" priority="1"
rgba="0.55 0.55 0.55 1" />
</default>
<default class="pad_box2">
<geom mass="0" type="box" pos="0 -0.0026 0.009375" size="0.011 0.004 0.009375"
friction="0.6"
solimp="0.95 0.99 0.001" solref="0.004 1" priority="1"
rgba="0.45 0.45 0.45 1" />
</default>
</default>
</default>
</default>
<asset>
<material name="black" rgba="0.033 0.033 0.033 1" />
<material name="jointgray" rgba="0.278 0.278 0.278 1" />
<material name="linkgray" rgba="0.82 0.82 0.82 1" />
<material name="urblue" rgba="0.49 0.678 0.8 1" />
<material name="cube_material" rgba="1.0 0.0 0.0 1" />
<material name="table_material" rgba="0.6 0.4 0.2 1" />
<mesh file="base_0.obj" />
<mesh file="base_1.obj" />
<mesh file="shoulder_0.obj" />
<mesh file="shoulder_1.obj" />
<mesh file="shoulder_2.obj" />
<mesh file="upperarm_0.obj" />
<mesh file="upperarm_1.obj" />
<mesh file="upperarm_2.obj" />
<mesh file="upperarm_3.obj" />
<mesh file="forearm_0.obj" />
<mesh file="forearm_1.obj" />
<mesh file="forearm_2.obj" />
<mesh file="forearm_3.obj" />
<mesh file="wrist1_0.obj" />
<mesh file="wrist1_1.obj" />
<mesh file="wrist1_2.obj" />
<mesh file="wrist2_0.obj" />
<mesh file="wrist2_1.obj" />
<mesh file="wrist2_2.obj" />
<mesh file="wrist3.obj" />
<!-- Gripper 2F85 -->
<material class="g2f85" name="metal" rgba="0.58 0.58 0.58 1" />
<material class="g2f85" name="silicone" rgba="0.1882 0.1882 0.1882 1" />
<material class="g2f85" name="gray" rgba="0.4627 0.4627 0.4627 1" />
<material class="g2f85" name="2f85_black" rgba="0.149 0.149 0.149 1" />
<mesh class="g2f85" file="base_mount.stl" />
<mesh class="g2f85" file="base.stl" />
<mesh class="g2f85" file="driver.stl" />
<mesh class="g2f85" file="coupler.stl" />
<mesh class="g2f85" file="follower.stl" />
<mesh class="g2f85" file="pad.stl" />
<mesh class="g2f85" file="silicone_pad.stl" />
<mesh class="g2f85" file="spring_link.stl" />
</asset>
<worldbody>
<body name="base" quat="1 0 0 1" childclass="ur5e">
<inertial mass="4" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072" />
<geom mesh="base_0" material="black" class="visual" />
<geom mesh="base_1" material="jointgray" class="visual" />
<body name="shoulder_link" pos="0 0 0.163">
<inertial mass="3.7" pos="0 0 0" diaginertia="0.0102675 0.0102675 0.00666" />
<joint name="shoulder_pan" axis="0 0 1" />
<geom mesh="shoulder_0" material="urblue" class="visual" />
<geom mesh="shoulder_1" material="black" class="visual" />
<geom mesh="shoulder_2" material="jointgray" class="visual" />
<geom class="collision" size="0.06 0.06" pos="0 0 -0.04" />
<body name="upper_arm_link" pos="0 0.138 0" quat="1 0 1 0">
<inertial mass="8.393" pos="0 0 0.2125" diaginertia="0.133886 0.133886 0.0151074" />
<joint name="shoulder_lift" />
<geom mesh="upperarm_0" material="linkgray" class="visual" />
<geom mesh="upperarm_1" material="black" class="visual" />
<geom mesh="upperarm_2" material="jointgray" class="visual" />
<geom mesh="upperarm_3" material="urblue" class="visual" />
<geom class="collision" pos="0 -0.04 0" quat="1 1 0 0" size="0.06 0.06" />
<geom class="collision" size="0.05 0.2" pos="0 0 0.2" />
<body name="forearm_link" pos="0 -0.131 0.425">
<inertial mass="2.275" pos="0 0 0.196" diaginertia="0.0311796 0.0311796 0.004095" />
<joint name="elbow" class="joint_limited" />
<geom mesh="forearm_0" material="urblue" class="visual" />
<geom mesh="forearm_1" material="linkgray" class="visual" />
<geom mesh="forearm_2" material="black" class="visual" />
<geom mesh="forearm_3" material="jointgray" class="visual" />
<geom class="collision" pos="0 0.08 0" quat="1 1 0 0" size="0.055 0.06" />
<geom class="collision" size="0.038 0.19" pos="0 0 0.2" />
<body name="wrist_1_link" pos="0 0 0.392" quat="1 0 1 0">
<inertial mass="1.219" pos="0 0.127 0" diaginertia="0.0025599 0.0025599 0.0021942" />
<joint name="wrist_1" />
<geom mesh="wrist1_0" material="black" class="visual" />
<geom mesh="wrist1_1" material="urblue" class="visual" />
<geom mesh="wrist1_2" material="jointgray" class="visual" />
<geom class="collision" pos="0 0.05 0" quat="1 1 0 0" size="0.04 0.07" />
<body name="wrist_2_link" pos="0 0.127 0">
<inertial mass="1.219" pos="0 0 0.1" diaginertia="0.0025599 0.0025599 0.0021942" />
<joint name="wrist_2" axis="0 0 1" />
<geom mesh="wrist2_0" material="black" class="visual" />
<geom mesh="wrist2_1" material="urblue" class="visual" />
<geom mesh="wrist2_2" material="jointgray" class="visual" />
<geom name="wrist_2_link" class="collision" size="0.04 0.06" pos="0 0 0.04" />
<body name="wrist_3_link" pos="0 0 0.1">
<inertial mass="0.1879" pos="0 0.0771683 0" quat="1 0 0 1"
diaginertia="0.000132134 9.90863e-05 9.90863e-05" />
<joint name="wrist_3" />
<geom material="linkgray" mesh="wrist3" class="visual" />
<geom name="wrist_3_link" class="collision" pos="0 0.025 0" quat="1 1 0 0"
size="0.04 0.05" />
<!-- BEGINNING OF 2F85 -->
<body name="attachment" pos="0 0.1 0" quat="-1 1 1 1">
<site name="attachment_site" size="0.01" pos="0 0 0.13" quat="-1 1 1 1"
rgba="1 0 0 1" group="1" />
<body name="base_mount" pos="0 0 0" childclass="g2f85">
<geom class="2f85_visual" mesh="base_mount" material="2f85_black" />
<geom class="2f85_collision" mesh="base_mount" />
<body name="g2f85_base" pos="0 0 0" quat="1 0 0 -1">
<inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675"
quat="1 0 0 0"
diaginertia="0.000260285 0.000225381 0.000152708" />
<geom class="2f85_visual" mesh="base" material="2f85_black" />
<geom class="2f85_collision" mesh="base" />
<site name="pinch" pos="0 0 0.145" type="sphere" group="5"
rgba="0.9 0.9 0.9 1"
size="0.005" />
<!-- Right-hand side 4-bar linkage -->
<body name="right_driver" pos="0 0.0306011 0.054904">
<inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314"
quat="0.681301 0.732003 0 0"
diaginertia="1.72352e-06 1.60906e-06 3.22006e-07" />
<joint name="right_driver_joint" class="driver" />
<geom class="2f85_visual" mesh="driver" material="gray" />
<geom class="2f85_collision" mesh="driver" />
<body name="right_coupler" pos="0 0.0315 -0.0041">
<inertial mass="0.0140974" pos="0 0.00301209 0.0232175"
quat="0.705636 -0.0455904 0.0455904 0.705636"
diaginertia="4.16206e-06 3.52216e-06 8.88131e-07" />
<joint name="right_coupler_joint" class="coupler" />
<geom class="2f85_visual" mesh="coupler" material="2f85_black" />
<geom class="2f85_collision" mesh="coupler" />
</body>
</body>
<body name="right_spring_link" pos="0 0.0132 0.0609">
<inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658"
quat="0.663403 -0.244737 0.244737 0.663403"
diaginertia="8.96853e-06 6.71733e-06 2.63931e-06" />
<joint name="right_spring_link_joint" class="spring_link" />
<geom class="2f85_visual" mesh="spring_link" material="2f85_black" />
<geom class="2f85_collision" mesh="spring_link" />
<body name="right_follower" pos="0 0.055 0.0375">
<inertial mass="0.0125222" pos="0 -0.011046 0.0124786"
quat="1 0.1664 0 0"
diaginertia="2.67415e-06 2.4559e-06 6.02031e-07" />
<joint name="right_follower_joint" class="follower" />
<geom class="2f85_visual" mesh="follower" material="2f85_black" />
<geom class="2f85_collision" mesh="follower" />
<body name="right_pad" pos="0 -0.0189 0.01352">
<geom class="pad_box1" name="right_pad1" />
<geom class="pad_box2" name="right_pad2" />
<inertial mass="0.0035" pos="0 -0.0025 0.0185"
quat="0.707107 0 0 0.707107"
diaginertia="4.73958e-07 3.64583e-07 1.23958e-07" />
<geom class="2f85_visual" mesh="pad" />
<body name="right_silicone_pad">
<geom name="right_driver" class="2f85_visual" mesh="silicone_pad" material="2f85_black" />
</body>
</body>
</body>
</body>
<!-- Left-hand side 4-bar linkage -->
<body name="left_driver" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
<inertial mass="0.00899563" pos="0 0.0177547 0.00107314"
quat="0.681301 0.732003 0 0"
diaginertia="1.72352e-06 1.60906e-06 3.22006e-07" />
<joint name="left_driver_joint" class="driver" />
<geom class="2f85_visual" mesh="driver" material="gray" />
<geom class="2f85_collision" mesh="driver" />
<body name="left_coupler" pos="0 0.0315 -0.0041">
<inertial mass="0.0140974" pos="0 0.00301209 0.0232175"
quat="0.705636 -0.0455904 0.0455904 0.705636"
diaginertia="4.16206e-06 3.52216e-06 8.88131e-07" />
<joint name="left_coupler_joint" class="coupler" />
<geom class="2f85_visual" mesh="coupler" material="2f85_black" />
<geom class="2f85_collision" mesh="coupler" />
</body>
</body>
<body name="left_spring_link" pos="0 -0.0132 0.0609" quat="0 0 0 1">
<inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658"
quat="0.663403 -0.244737 0.244737 0.663403"
diaginertia="8.96853e-06 6.71733e-06 2.63931e-06" />
<joint name="left_spring_link_joint" class="spring_link" />
<geom class="2f85_visual" mesh="spring_link" material="2f85_black" />
<geom class="2f85_collision" mesh="spring_link" />
<body name="left_follower" pos="0 0.055 0.0375">
<inertial mass="0.0125222" pos="0 -0.011046 0.0124786"
quat="1 0.1664 0 0"
diaginertia="2.67415e-06 2.4559e-06 6.02031e-07" />
<joint name="left_follower_joint" class="follower" />
<geom class="2f85_visual" mesh="follower" material="2f85_black" />
<geom class="2f85_collision" mesh="follower" />
<body name="left_pad" pos="0 -0.0189 0.01352">
<geom class="pad_box1" name="left_pad1" />
<geom class="pad_box2" name="left_pad2" />
<inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1"
diaginertia="4.73958e-07 3.64583e-07 1.23958e-07" />
<geom class="2f85_visual" mesh="pad" />
<body name="left_silicone_pad">
<geom class="2f85_visual" mesh="silicone_pad" material="2f85_black" />
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<!-- END OF 2F85 -->
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<!-- Table -->
<body name="table" pos="0.4 0 0.1">
<inertial mass="10.0" pos="0 0 0" diaginertia="0.1 0.1 0.1" />
<geom name="table" type="box" size="0.25 0.25 0.1" material="table_material"
contype="1" conaffinity="1" friction="1.0 0.5 0.5" />
</body>
<!-- Pickable target cube -->
<body name="target_cube" pos="0.4 0.0 0.25">
<freejoint />
<inertial mass="0.005" pos="0 0 0" diaginertia="0.00005 0.00005 0.00005" />
<geom name="target_cube" type="box" size="0.02 0.02 0.02" material="cube_material"
friction="1.0 0.5 0.5" contype="1" conaffinity="1" />
</body>
<!-- Visual target marker for IK -->
<body name="target_marker" pos="0.5 0.15 0.35" quat="1 0 0 0" mocap="true">
<geom type="box" size="0.02 0.02 0.02" contype="0" conaffinity="0" rgba="0.6 0.3 0.3 0.3" />
</body>
</worldbody>
<contact>
<exclude body1="base" body2="left_driver"/>
<exclude body1="base" body2="right_driver"/>
<exclude body1="base" body2="left_spring_link"/>
<exclude body1="base" body2="right_spring_link"/>
<exclude body1="right_coupler" body2="right_follower"/>
<exclude body1="left_coupler" body2="left_follower"/>
</contact>
<tendon>
<fixed name="split">
<joint joint="right_driver_joint" coef="0.5" />
<joint joint="left_driver_joint" coef="0.5" />
</fixed>
</tendon>
<equality>
<connect anchor="0 0 0" body1="right_follower" body2="right_coupler"
solimp="0.95 0.99 0.001" solref="0.005 1" />
<connect anchor="0 0 0" body1="left_follower" body2="left_coupler" solimp="0.95 0.99 0.001"
solref="0.005 1" />
<joint joint1="right_driver_joint" joint2="left_driver_joint" polycoef="0 1 0 0 0"
solimp="0.95 0.99 0.001"
solref="0.005 1" />
</equality>
<actuator>
<position class="position" name="shoulder_pan" joint="shoulder_pan" />
<position class="position" name="shoulder_lift" joint="shoulder_lift" />
<position class="position_limited" name="elbow" joint="elbow" />
<position class="position_small" name="wrist_1" joint="wrist_1" />
<position class="position_small" name="wrist_2" joint="wrist_2" />
<position class="position_small" name="wrist_3" joint="wrist_3" />
<general class="g2f85" name="fingers_actuator" tendon="split" forcerange="-15 15"
ctrlrange="0 255"
gainprm="0.8 0 0" biasprm="0 -70 -7" />
</actuator>
<keyframe>
<key name="home"
qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 0 0 0 0 0 0 0 0 0.4 0.0 0.25 1 0 0 0"
ctrl="0 0 0 0 0 0 0" />
</keyframe>
</mujoco>
import time
import mujoco
import mujoco.viewer
import numpy as np
from loop_rate_limiters import RateLimiter
import mink
model_path = "scene2.xml"
def get_cube_position(model, data):
"""Get the current position of the target cube"""
try:
cube_body_id = model.body("target_cube").id
print(f"Debug: cube_body_id = {cube_body_id}")
cube_pos = data.xpos[cube_body_id].copy()
print(f"Debug: raw cube position = {cube_pos}")
return cube_pos
except Exception as e:
print(f"Error getting cube position: {e}")
# Fallback to XML position
return np.array([0.49, 0.05, 0.23], dtype=np.float64)
def run_pick_and_place():
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)
# =================== #
# Setup IK
# =================== #
configuration = mink.Configuration(model)
end_effector_task = mink.FrameTask(
frame_name="attachment_site", # EE site in your URDF/XML
frame_type="site",
position_cost=1.0,
orientation_cost=1.0,
lm_damping=1e-6,
)
posture_task = mink.PostureTask(model, cost=1e-3)
tasks = [end_effector_task, posture_task]
limits = [
mink.ConfigurationLimit(model=configuration.model),
]
# Add joint velocity limits
max_velocities = {
"shoulder_pan": np.pi,
"shoulder_lift": np.pi,
"elbow": np.pi,
"wrist_1": np.pi,
"wrist_2": np.pi,
"wrist_3": np.pi,
}
limits.append(mink.VelocityLimit(model, max_velocities))
# IK solver settings
solver = "daqp"
pos_threshold = 1e-3
ori_threshold = 1e-3
max_iters = 5
# =================== #
# Waypoints - will be defined dynamically based on cube position
# =================== #
quat = np.array([0.7071, 0.0, 0.7071, 0.0], dtype=np.float64) # 90° rotation around Y-axis to point down
# =================== #
# Run viewer loop
# =================== #
with mujoco.viewer.launch_passive(
model=model,
data=data,
show_left_ui=True,
show_right_ui=True,
) as viewer:
mujoco.mj_resetDataKeyframe(model, data, model.key("home").id)
configuration.update(data.qpos)
mujoco.mj_forward(model, data)
# Print initial end effector position
site_id = model.site("attachment_site").id
initial_ee_pos = data.site_xpos[site_id].copy()
print(f"Initial end effector position: [{initial_ee_pos[0]:.3f}, {initial_ee_pos[1]:.3f}, {initial_ee_pos[2]:.3f}]")
# Get cube position and create waypoints based on it
cube_pos = get_cube_position(model, data)
print(f"Initial cube position (before physics): [{cube_pos[0]:.3f}, {cube_pos[1]:.3f}, {cube_pos[2]:.3f}]")
# Run a few simulation steps to let physics settle
for _ in range(100):
mujoco.mj_step(model, data)
cube_pos = get_cube_position(model, data)
print(f"Cube position (after physics settle): [{cube_pos[0]:.3f}, {cube_pos[1]:.3f}, {cube_pos[2]:.3f}]")
# Define waypoints based on cube position
waypoints = [
np.array([cube_pos[0], cube_pos[1], cube_pos[2] + 0.15], dtype=np.float64), # approach above cube
np.array([cube_pos[0], cube_pos[1], cube_pos[2] + 0.02], dtype=np.float64), # move down to pick
np.array([cube_pos[0], cube_pos[1], cube_pos[2] + 0.15], dtype=np.float64), # lift cube
np.array([0.3, -0.2, cube_pos[2] + 0.15], dtype=np.float64), # move to place location
np.array([0.3, -0.2, cube_pos[2] + 0.05], dtype=np.float64), # lower to place
]
posture_task.set_target_from_configuration(configuration)
rate = RateLimiter(frequency=200.0, warn=False)
for i, pos in enumerate(waypoints):
print(f"Moving to waypoint {i+1}: {pos}")
# Set the target marker position for visualization
data.mocap_pos[0] = pos # Set mocap position to waypoint
data.mocap_quat[0, :] = quat # Set mocap orientation
# Create SE3 transform for the target using target_marker
T_target = mink.SE3.from_mocap_name(model, data, "target_marker")
end_effector_task.set_target(T_target)
# Iteratively solve IK until converged
reached = False
iteration_count = 0
max_outer_iters = 20 # Prevent infinite loop
while not reached and viewer.is_running() and iteration_count < max_outer_iters:
for _ in range(max_iters):
vel = mink.solve_ik(
configuration, tasks, rate.dt, solver, limits=limits
)
configuration.integrate_inplace(vel, rate.dt)
err = end_effector_task.compute_error(configuration)
#pos_ok = np.linalg.norm(err[:3]) <= pos_threshold
#ori_ok = np.linalg.norm(err[3:]) <= ori_threshold
# Debug info every 100 iterations
# if iteration_count % 100 == 0:
# print(f" Iteration {iteration_count}: pos_error={np.linalg.norm(err[:3]):.4f}, ori_error={np.linalg.norm(err[3:]):.4f}")
# if pos_ok and ori_ok:
# reached = True
# print(f" ✅ Reached target in {iteration_count} iterations")
# break
# Update simulation with new joint positions
data.qpos[:6] = configuration.q[:6] # Update joint positions
data.ctrl[:6] = configuration.q[:6] # Update control
mujoco.mj_step(model, data)
viewer.sync()
rate.sleep()
time.sleep(0.05)
iteration_count += 1
# if not reached:
# print(f" ⚠️ Could not reach target after {iteration_count} iterations")
# Print current end effector position after waypoint
site_id = model.site("attachment_site").id
current_ee_pos = data.site_xpos[site_id].copy()
print(f" End effector position: [{current_ee_pos[0]:.3f}, {current_ee_pos[1]:.3f}, {current_ee_pos[2]:.3f}]")
print(f" Target position: [{pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}]")
distance_error = np.linalg.norm(current_ee_pos - pos)
print(f" Distance error: {distance_error:.4f}m")
print()
#Simple open/close gripper events
if i == 1: # at pick (waypoint 2)
print("🔽 Closing gripper to pick up cube...")
print(model.actuator("fingers_actuator").id)
data.ctrl[6] = 255.0 # Close gripper
# Give time for gripper to close
for _ in range(300):
mujoco.mj_step(model, data)
viewer.sync()
rate.sleep()
#time.sleep(0.02)
for _ in range(300):
mujoco.mj_step(model, data)
viewer.sync()
rate.sleep()
print("✅ Cube picked up!")
# if i == 1: # at pick (waypoint 2)
# print("🔽 Gradually closing gripper to pick up cube...")
# # Gradual gripper closing
# for close_step in range(10):
# grip_force = (close_step + 1) * 25.5 # Gradually increase from 25.5 to 255
# data.ctrl[6] = grip_force
# print(f" Gripper force: {grip_force:.1f}")
# # Give time for each step
# for _ in range(30):
# mujoco.mj_step(model, data)
# viewer.sync()
# rate.sleep()
# time.sleep(0.01)
# # Hold the grip
# print(" Maintaining grip...")
# for _ in range(100):
# mujoco.mj_step(model, data)
# viewer.sync()
# rate.sleep()
# print("✅ Cube gripped!")
elif i == 4: # at place (waypoint 5)
print("🔼 Opening gripper to release cube...")
data.ctrl[6] = 0.0 # Open gripper
# Give time for gripper to open
for _ in range(300):
mujoco.mj_step(model, data)
viewer.sync()
rate.sleep()
print("✅ Cube released!")
print("✅ Pick and place operation completed!")
print("The cube has been moved from its original position.")
time.sleep(3)
if __name__ == "__main__":
run_pick_and_place()
I'm new to this mujoco world, please help to look into the target_cube in the ur5e.xml for the cube properties and the gripper control part in the script. Your advice is highly appreciated. Thank you.