# Merge the actual gripper as a child of the dummy body
world.merge(gripper, merge_body="gripper_base")
# Create a new actuator to control our slider joint
world.actuator.append(
new_actuator(joint="gripper_z_joint",
act_type="position",
name="gripper_z",
kp="500"))
# add an object for grasping
mujoco_object = BoxObject(name="box",
size=[0.02, 0.02, 0.02],
rgba=[1, 0, 0, 1],
friction=[1, 0.005, 0.0001]).get_obj()
# Set the position of this object
mujoco_object.set("pos", "0 0 0.11")
# Add our object to the world body
world.worldbody.append(mujoco_object)
# add reference objects for x and y axes
x_ref = BoxObject(name="x_ref",
size=[0.01, 0.01, 0.01],
rgba=[0, 1, 0, 1],
obj_type="visual",
joints=None).get_obj()
x_ref.set("pos", "0.2 0 0.105")
world.worldbody.append(x_ref)
y_ref = BoxObject(name="y_ref",
size=[0.01, 0.01, 0.01],
rgba=[0, 0, 1, 1],
obj_type="visual",
def __init__(
self,
gripper,
pos,
quat,
gripper_low_pos,
gripper_high_pos,
box_size=None,
box_density=10000,
step_time=400,
render=True,
):
# define viewer
self.viewer = None
world = MujocoWorldBase()
# Add a table
arena = TableArena(table_full_size=(0.4, 0.4, 0.1), table_offset=(0, 0, 0.1), has_legs=False)
world.merge(arena)
# Add a gripper
self.gripper = gripper
# Create another body with a slider joint to which we'll add this gripper
gripper_body = ET.Element("body")
gripper_body.set("pos", pos)
gripper_body.set("quat", quat) # flip z
gripper_body.append(new_joint(name="gripper_z_joint", type="slide", axis="0 0 -1", damping="50"))
# Add all gripper bodies to this higher level body
for body in gripper.worldbody:
gripper_body.append(body)
# Merge the all of the gripper tags except its bodies
world.merge(gripper, merge_body=None)
# Manually add the higher level body we created
world.worldbody.append(gripper_body)
# Create a new actuator to control our slider joint
world.actuator.append(new_actuator(joint="gripper_z_joint", act_type="position", name="gripper_z", kp="500"))
# Add an object for grasping
# density is in units kg / m3
TABLE_TOP = [0, 0, 0.09]
if box_size is None:
box_size = [0.02, 0.02, 0.02]
box_size = np.array(box_size)
self.cube = BoxObject(
name="object", size=box_size, rgba=[1, 0, 0, 1], friction=[1, 0.005, 0.0001], density=box_density
)
object_pos = np.array(TABLE_TOP + box_size * [0, 0, 1])
mujoco_object = self.cube.get_obj()
# Set the position of this object
mujoco_object.set("pos", array_to_string(object_pos))
# Add our object to the world body
world.worldbody.append(mujoco_object)
# add reference objects for x and y axes
x_ref = BoxObject(
name="x_ref", size=[0.01, 0.01, 0.01], rgba=[0, 1, 0, 1], obj_type="visual", joints=None
).get_obj()
x_ref.set("pos", "0.2 0 0.105")
world.worldbody.append(x_ref)
y_ref = BoxObject(
name="y_ref", size=[0.01, 0.01, 0.01], rgba=[0, 0, 1, 1], obj_type="visual", joints=None
).get_obj()
y_ref.set("pos", "0 0.2 0.105")
world.worldbody.append(y_ref)
self.world = world
self.render = render
self.simulation_ready = False
self.step_time = step_time
self.cur_step = 0
if gripper_low_pos > gripper_high_pos:
raise ValueError(
"gripper_low_pos {} is larger " "than gripper_high_pos {}".format(gripper_low_pos, gripper_high_pos)
)
self.gripper_low_pos = gripper_low_pos
self.gripper_high_pos = gripper_high_pos
# Create a new actuator to control our slider joint
world.actuator.append(
new_actuator(joint="gripper_z_joint",
act_type="position",
name="gripper_z",
kp="500"))
# add an object for grasping
mujoco_object = BoxObject(
name="box",
size=[0.02, 0.02, 0.02], # 2cm
rgba=[1, 0, 0, 1],
friction=[1, 0.005, 0.0001]).get_obj()
# Set the position of this object
mujoco_object.set("pos", "0 0 0.11") # 11cm up wrt center of object?
# Add our object to the world body
world.worldbody.append(mujoco_object)
# add reference objects for x and y axes
x_ref = BoxObject(name="x_ref",
size=[0.01, 0.01, 0.01],
rgba=[0, 1, 0, 1],
obj_type="visual",
joints=None).get_obj()
x_ref.set("pos", "0.2 0 0.105")
world.worldbody.append(x_ref)
y_ref = BoxObject(name="y_ref",
size=[0.01, 0.01, 0.01],
