Motor Actuator in MuJoCo not working for me

Discussion in 'Simulation' started by Pravin, Apr 1, 2018.

  1. Hi,

    I am trying to use MuJoCo Py to lift up a robot gripper after it has grasped an object using several motor actuators. Here is the Model XML I am using. I am using 3 slide joints to control the movement of the robot gripper in x, y, and z directions. I am also using 2 regular joints to control the left and right fingers of the gripper individually.

    The 5 joints I am attempting to control using actuators are bolded in the following XML text that I am using.

    MODEL_XML =
    """<?xml version="1.0" encoding="utf-8"?>
    <mujoco>

    <worldbody>
    <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
    <geom size="1 1 0.1" type="box" mass="0" material="table_mat"></geom>

    <body name = "object" pos="x y z">
    <joint type="free"/>
    <geom type="mesh" mesh="OBJECT"/>
    </body>

    <body childclass="robot0:fetch" name="robot0:base_link" pos="-0.5 0.2 0.5" quat="0.707 0 0.707 0">
    <joint axis="1 0 0" damping="1e+11" name="robot0:slide0" pos="0 0 0" type="slide"></joint>
    <joint axis="0 1 0" damping="1e+11" name="robot0:slide1" pos="0 0 0" type="slide"></joint>
    <joint axis="0 0 1" damping="1e+11" name="robot0:slide2" pos="0 0 0" type="slide"></joint>
    <inertial diaginertia="1.2869 1.2236 0.9868" mass="70.1294" pos="-0.0036 0 0.0014" quat="0.7605 -0.0133 -0.0061 0.6491"></inertial>
    <body name="robot0:wrist_roll_link" pos="0.1385 0 0">
    <inertial diaginertia="0.0001 0.0001 0.0001" mass="0.1354" pos="0.0095 0.0004 -0.0002"></inertial>
    <joint axis="1 0 0" limited="false" name="robot0:wrist_roll_joint"></joint>
    <geom mesh="robot0:wrist_roll_link" name="robot0:wrist_roll_link" material="robot0:arm_mat"></geom>
    <body euler="0 0 0" name="robot0:gripper_link" pos="0.1664 0 0">
    <inertial diaginertia="0.0024 0.0019 0.0013" mass="1.5175" quat="0 0.7071 0 0.7071"></inertial>
    <geom mesh="robot0:gripper_link" name="robot0:gripper_link" material="robot0:gripper_mat"></geom>

    <body childclass="robot0:fetchGripper" name="robot0:r_gripper_finger_link" pos="0 0.0159 0">
    <inertial diaginertia="0.1 0.1 0.1" mass="4" pos="-0.01 0 0"></inertial>
    <joint type="slide" axis="0 1 0" name="robot0:r_gripper_finger_joint" range="0 0.05"></joint>
    <geom pos="0 -0.008 0" size="0.0385 0.007 0.0135" type="box" name="robot0:r_gripper_finger_link" ></geom>
    </body>

    <body childclass="robot0:fetchGripper" name="robot0:l_gripper_finger_link" pos="0 -0.0159 0">
    <inertial diaginertia="0.1 0.1 0.1" mass="4" pos="-0.01 0 0"></inertial>
    <joint type="slide" axis="0 -1 0" name="robot0:l_gripper_finger_joint" range="0 0.05"></joint>
    <geom pos="0 0.008 0" size="0.0385 0.007 0.0135" type="box" name="robot0:l_gripper_finger_link"></geom>
    </body>

    <site name="robot0:grip" pos="0.02 0 0" rgba="0 0 0 0" size="0.02 0.02 0.02"></site>
    </body>
    </body>
    </body>

    </worldbody>
    <actuator>
    <motor ctrlrange="0 0.2" joint="robot0:l_gripper_finger_joint" name="robot0:l_gripper_finger_joint"/>
    <motor ctrlrange="0 0.2" joint="robot0:r_gripper_finger_joint" name="robot0:r_gripper_finger_joint"/>

    <motor ctrlrange="-0.2 0.2" joint="robot0:slide0" name="robot0:slide0"/>
    <motor ctrlrange="-0.2 0.2" joint="robot0:slide1" name="robot0:slide1"/>
    <motor ctrlrange="-0.2 0.2" joint="robot0:slide2" name="robot0:slide2"/>
    </actuator>
    </mujoco>"""


    In the main loop of my Python program, I am attempting to move the gripper once it has grasped an object by supplying control inputs to the actuator (at time step 5000). However, when I try this out in MuJoCoPy, the gripper does not move as a result of changing the actuator input.

    model = load_model_from_path("graspingMarkup.xml")
    sim = MjSim(model)
    sim.data.set_joint_qpos('robot0:l_gripper_finger_joint', 0.05)
    sim.data.set_joint_qpos('robot0:r_gripper_finger_joint', 0.05)
    sim.forward()
    sim.step()
    viewer = MjViewer(sim)
    t = 0
    while True:
    viewer.render()
    if t < 2000: # at all time steps less than 2000, open the gripper fingers
    sim.data.set_joint_qpos('robot0:l_gripper_finger_joint', 0.05)
    sim.data.set_joint_qpos('robot0:r_gripper_finger_joint', 0.05)

    if t == 2000: # at time step 2000, move the gripper to the object
    sim.data.set_joint_qpos('robot0:slide2', sim.data.get_body_xpos('object')[0] - sim.data.get_body_xpos('robot0:gripper_link')[0])
    sim.data.set_joint_qpos('robot0:slide1', sim.data.get_body_xpos('object')[1] - sim.data.get_body_xpos('robot0:gripper_link')[1])
    sim.data.set_joint_qpos('robot0:slide0', sim.data.get_body_xpos('robot0:gripper_link')[2] - sim.data.get_body_xpos('object')[2])

    if t == 3000: # at all time step 3000, close the gripper so that it grasps the object
    sim.data.set_joint_qpos('robot0:l_gripper_finger_joint', 0.005)
    sim.data.set_joint_qpos('robot0:r_gripper_finger_joint', 0.005)

    if t > 5000: # at time step 5000, lift the gripper body up by supplying massive control actuator inputs
    sim.data.ctrl[:] = np.array([1000, 1000, 1000, 1000, 1000])

    Could I please get help on this?

    Thank You.






     
  2. Can anyone help me with this please?

    Thank you,
    Pravin