def _load_model(self): """ Loads an xml model, puts it in self.model """ super()._load_model() # Adjust base pose accordingly xpos = self.robots[0].robot_model.base_xpos_offset["table"]( self.table_full_size[0]) self.robots[0].robot_model.set_base_xpos(xpos) # load model for table top workspace mujoco_arena = TableArena( table_full_size=self.table_full_size, table_friction=self.table_friction, table_offset=self.table_offset, ) # Arena always gets set to zero origin mujoco_arena.set_origin([0, 0, 0]) # initialize objects of interest self.cube = BoxObject( name="cube", size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015], size_max=[0.022, 0.022, 0.022], # [0.018, 0.018, 0.018]) rgba=[1, 0, 0, 1], ) self.goal = BoxObject( name="goal", size_min=[0.005, 0.005, 0.005], size_max=[0.005, 0.005, 0.005], rgba=[0, 0, 1, 1], density=11342, # Density of lead such that the goal is hard to move ) # Create placement initializer if self.placement_initializer is not None: self.placement_initializer.reset() self.placement_initializer.add_objects([self.cube, self.goal]) else: self.placement_initializer = UniformRandomSampler( name="ObjectSampler", mujoco_objects=[self.cube, self.goal], x_range=[-0.20, 0.20], y_range=[-0.20, 0.20], rotation=None, ensure_object_boundary_in_range=False, ensure_valid_placement=True, reference_pos=self.table_offset, z_offset=0.01, ) # task includes arena, robot, and objects of interest self.model = ManipulationTask( mujoco_arena=mujoco_arena, mujoco_robots=[robot.robot_model for robot in self.robots], mujoco_objects=[self.cube, self.goal], )
def _load_model(self): """ Loads an xml model, puts it in self.model """ super()._load_model() self.mujoco_robot.set_base_xpos([0, 0, 0]) # load model for table top workspace self.mujoco_arena = TableArena(table_full_size=self.table_full_size, table_friction=self.table_friction) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # The sawyer robot has a pedestal, we want to align it with the table self.mujoco_arena.set_origin( [0.16 + self.table_full_size[0] / 2, 0, 0]) # initialize objects of interest cube = BoxObject( size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015], size_max=[0.020, 0.020, 0.020], # [0.018, 0.018, 0.018]) rgba=[1, 0, 0, 1], ) cube_ = BoxObject( size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015], size_max=[0.020, 0.020, 0.020], # [0.018, 0.018, 0.018]) rgba=[1, 0, 0, 1], ) cube1 = BoxObject( size_min=[0.120, 0.010, 0.080], # [0.015, 0.015, 0.015], size_max=[0.120, 0.010, 0.080], # [0.018, 0.018, 0.018]) rgba=[0, 1, 0, 1], ) cube2 = BoxObject( size_min=[0.120, 0.010, 0.080], # [0.015, 0.015, 0.015], size_max=[0.120, 0.010, 0.080], # [0.018, 0.018, 0.018]) rgba=[0, 1, 0, 1], ) self.mujoco_objects = OrderedDict([("cube", cube), ("cube_", cube_), ("cube1", cube1), ("cube2", cube2)]) self.n_objects = len(self.mujoco_objects) # task includes arena, robot, and objects of interest self.model = TableTopTask( self.mujoco_arena, self.mujoco_robot, self.mujoco_objects, initializer=self.placement_initializer, ) self.model.place_objects()
def _load_model(self): """ Loads an xml model, puts it in self.model """ super()._load_model() self.mujoco_robot.set_base_xpos([0, 0, 0]) # load model for table top workspace self.mujoco_arena = TableArena(table_full_size=self.table_full_size, table_friction=self.table_friction) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # The sawyer robot has a pedestal, we want to align it with the table self.mujoco_arena.set_origin( [0.16 + self.table_full_size[0] / 2, 0, 0]) # initialize objects of interest cube = BoxObject( size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015], size_max=[0.022, 0.022, 0.022], # [0.018, 0.018, 0.018]) rgba=[1, 0, 0, 1], ) if self.target_object == 'can': cube = CanObject() elif self.target_object == 'bottle': cube = BottleObject() elif self.target_object == 'milk': cube = MilkObject() elif self.target_object == 'cereal': cube = CerealObject() elif self.target_object == 'lemon': cube = LemonObject() self.mujoco_objects = OrderedDict([("cube", cube)]) # task includes arena, robot, and objects of interest self.model = TableTopTask( self.mujoco_arena, self.mujoco_robot, self.mujoco_objects, initializer=self.placement_initializer, ) self.model.place_objects()
def _load_model(self): """ Loads an xml model, puts it in self.model """ SingleArmEnv._load_model(self) # Adjust base pose accordingly xpos = self.robots[0].robot_model.base_xpos_offset["table"]( self.table_full_size[0]) self.robots[0].robot_model.set_base_xpos(xpos) # load model for table top workspace mujoco_arena = TableArena( table_full_size=self.table_full_size, table_friction=self.table_friction, table_offset=self.table_offset, ) # Arena always gets set to zero origin mujoco_arena.set_origin([0, 0, 0]) cube_material = self._get_cube_material() self.cube = BoxObject( name="cube", size_min=(0.025, 0.025, 0.025), size_max=(0.025, 0.025, 0.025), rgba=[1, 0, 0, 1], material=cube_material, ) self.placement_initializer.reset() self.placement_initializer.add_objects(self.cube) # task includes arena, robot, and objects of interest self.model = ManipulationTask( mujoco_arena=mujoco_arena, mujoco_robots=[robot.robot_model for robot in self.robots], mujoco_objects=self.cube, )
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
class GripperTester: """ A class that is used to test gripper Args: gripper (GripperModel): A gripper instance to be tested pos (str): (x y z) position to place the gripper in string form, e.g. '0 0 0.3' quat (str): rotation to apply to gripper in string form, e.g. '0 0 1 0' to flip z axis gripper_low_pos (float): controls the gipper y position, larger -> higher gripper_high_pos (float): controls the gipper y high position larger -> higher, must be larger than gripper_low_pos box_size (None or 3-tuple of int): the size of the box to grasp, None defaults to [0.02, 0.02, 0.02] box_density (int): the density of the box to grasp step_time (int): the interval between two gripper actions render (bool): if True, show rendering """ 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 def start_simulation(self): """ Starts simulation of the test world """ model = self.world.get_model(mode="mujoco_py") self.sim = MjSim(model) if self.render: self.viewer = MjViewer(self.sim) self.sim_state = self.sim.get_state() # For gravity correction gravity_corrected = ["gripper_z_joint"] self._gravity_corrected_qvels = [self.sim.model.get_joint_qvel_addr(x) for x in gravity_corrected] self.gripper_z_id = self.sim.model.actuator_name2id("gripper_z") self.gripper_z_is_low = False self.gripper_actuator_ids = [self.sim.model.actuator_name2id(x) for x in self.gripper.actuators] self.gripper_is_closed = True self.object_id = self.sim.model.body_name2id(self.cube.root_body) object_default_pos = self.sim.data.body_xpos[self.object_id] self.object_default_pos = np.array(object_default_pos, copy=True) self.reset() self.simulation_ready = True def reset(self): """ Resets the simulation to the initial state """ self.sim.set_state(self.sim_state) self.cur_step = 0 def close(self): """ Close the viewer if it exists """ if self.viewer is not None: self.viewer.close() def step(self): """ Forward the simulation by one timestep Raises: RuntimeError: if start_simulation is not yet called. """ if not self.simulation_ready: raise RuntimeError("Call start_simulation before calling step") if self.gripper_z_is_low: self.sim.data.ctrl[self.gripper_z_id] = self.gripper_low_pos else: self.sim.data.ctrl[self.gripper_z_id] = self.gripper_high_pos if self.gripper_is_closed: self._apply_gripper_action(1) else: self._apply_gripper_action(-1) self._apply_gravity_compensation() self.sim.step() if self.render: self.viewer.render() self.cur_step += 1 def _apply_gripper_action(self, action): """ Applies binary gripper action Args: action (int): Action to apply. Should be -1 (open) or 1 (closed) """ gripper_action_actual = self.gripper.format_action(np.array([action])) # rescale normalized gripper action to control ranges ctrl_range = self.sim.model.actuator_ctrlrange[self.gripper_actuator_ids] bias = 0.5 * (ctrl_range[:, 1] + ctrl_range[:, 0]) weight = 0.5 * (ctrl_range[:, 1] - ctrl_range[:, 0]) applied_gripper_action = bias + weight * gripper_action_actual self.sim.data.ctrl[self.gripper_actuator_ids] = applied_gripper_action def _apply_gravity_compensation(self): """ Applies gravity compensation to the simulation """ self.sim.data.qfrc_applied[self._gravity_corrected_qvels] = self.sim.data.qfrc_bias[ self._gravity_corrected_qvels ] def loop(self, total_iters=1, test_y=False, y_baseline=0.01): """ Performs lower, grip, raise and release actions of a gripper, each separated with T timesteps Args: total_iters (int): Iterations to perform before exiting test_y (bool): test if object is lifted y_baseline (float): threshold for determining that object is lifted """ seq = [(False, False), (True, False), (True, True), (False, True)] for cur_iter in range(total_iters): for cur_plan in seq: self.gripper_z_is_low, self.gripper_is_closed = cur_plan for step in range(self.step_time): self.step() if test_y: if not self.object_height > y_baseline: raise ValueError( "object is lifed by {}, ".format(self.object_height) + "not reaching the requirement {}".format(y_baseline) ) @property def object_height(self): """ Queries the height (z) of the object compared to on the ground Returns: float: Object height relative to default (ground) object position """ return self.sim.data.body_xpos[self.object_id][2] - self.object_default_pos[2]
def _load_model(self): """ Loads an xml model, puts it in self.model """ super()._load_model() # Adjust base pose accordingly xpos = self.robots[0].robot_model.base_xpos_offset["table"]( self.table_full_size[0]) self.robots[0].robot_model.set_base_xpos(xpos) # load model for table top workspace mujoco_arena = TableArena( table_full_size=self.table_full_size, table_friction=self.table_friction, table_offset=self.table_offset, ) # Arena always gets set to zero origin mujoco_arena.set_origin([0, 0, 0]) # initialize objects of interest tex_attrib = { "type": "cube", } mat_attrib = { "texrepeat": "1 1", "specular": "0.4", "shininess": "0.1", } redwood = CustomMaterial( texture="WoodRed", tex_name="redwood", mat_name="redwood_mat", tex_attrib=tex_attrib, mat_attrib=mat_attrib, ) greenwood = CustomMaterial( texture="WoodGreen", tex_name="greenwood", mat_name="greenwood_mat", tex_attrib=tex_attrib, mat_attrib=mat_attrib, ) self.cubeA = BoxObject( name="cubeA", size_min=[0.02, 0.02, 0.02], size_max=[0.02, 0.02, 0.02], rgba=[1, 0, 0, 1], material=redwood, ) self.cubeB = BoxObject( name="cubeB", size_min=[0.025, 0.025, 0.025], size_max=[0.025, 0.025, 0.025], rgba=[0, 1, 0, 1], material=greenwood, ) cubes = [self.cubeA, self.cubeB] # Create placement initializer if self.placement_initializer is not None: self.placement_initializer.reset() self.placement_initializer.add_objects(cubes) else: self.placement_initializer = UniformRandomSampler( name="ObjectSampler", mujoco_objects=cubes, x_range=[-0.08, 0.08], y_range=[-0.08, 0.08], rotation=None, ensure_object_boundary_in_range=False, ensure_valid_placement=True, reference_pos=self.table_offset, z_offset=0.01, ) # task includes arena, robot, and objects of interest self.model = ManipulationTask( mujoco_arena=mujoco_arena, mujoco_robots=[robot.robot_model for robot in self.robots], mujoco_objects=cubes, )
def __init__( self, name, box1_size=(0.025, 0.025, 0.025), box2_size=(0.025, 0.025, 0.0125), use_texture=True, ): # Set box sizes self.box1_size = np.array(box1_size) self.box2_size = np.array(box2_size) # Set texture attributes self.use_texture = use_texture self.box1_material = None self.box2_material = None self.box1_rgba = RED self.box2_rgba = BLUE # Define materials we want to use for this object if self.use_texture: # Remove RGBAs self.box1_rgba = None self.box2_rgba = None # Set materials for each box tex_attrib = { "type": "cube", } mat_attrib = { "texrepeat": "3 3", "specular": "0.4", "shininess": "0.1", } self.box1_material = CustomMaterial( texture="WoodRed", tex_name="box1_tex", mat_name="box1_mat", tex_attrib=tex_attrib, mat_attrib=mat_attrib, ) self.box2_material = CustomMaterial( texture="WoodBlue", tex_name="box2_tex", mat_name="box2_mat", tex_attrib=tex_attrib, mat_attrib=mat_attrib, ) # Create objects objects = [] for i, (size, mat, rgba) in enumerate( zip( (self.box1_size, self.box2_size), (self.box1_material, self.box2_material), (self.box1_rgba, self.box2_rgba), )): objects.append( BoxObject( name=f"box{i + 1}", size=size, rgba=rgba, material=mat, )) # Also add hinge for visualization objects.append( CylinderObject( name="hinge", size=np.array([ min(self.box1_size[2], self.box2_size[2]) / 5.0, min(self.box1_size[0], self.box2_size[0]) ]), rgba=[0.5, 0.5, 0, 1], obj_type="visual", )) # Define hinge joint rel_hinge_pos = [self.box2_size[0], 0, -self.box2_size[2] ] # want offset in all except y-axis hinge_joint = { "name": "box_hinge", "type": "hinge", "axis": "0 1 0", # y-axis hinge "pos": array_to_string(rel_hinge_pos), "stiffness": "0.0001", "limited": "true", "range": "0 1.57", } # Define positions -- second box should lie on top of first box with edge aligned at hinge joint # Hinge visualizer should be aligned at hinge joint location positions = [ np.zeros(3), # First box is centered at top-level body anyways np.array([ -(self.box2_size[0] - self.box1_size[0]), 0, self.box1_size[2] + self.box2_size[2] ]), np.array(rel_hinge_pos), ] quats = [ None, # Default quaternion for box 1 None, # Default quaternion for box 2 [0.707, 0.707, 0, 0], # Rotated 90 deg about x-axis ] # Define parents -- which body each is aligned to parents = [ None, # box 1 attached to top-level body objects[0].root_body, # box 2 attached to box 1 objects[1].root_body, # hinge attached to box 2 ] # Run super init super().__init__( name=name, objects=objects, object_locations=positions, object_quats=quats, object_parents=parents, body_joints={objects[1].root_body: [hinge_joint]}, )
def _load_model(self): """ Loads an xml model, puts it in self.model """ super()._load_model() # Verify the correct robot has been loaded assert isinstance(self.robots[0], SingleArm), \ "Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0])) # Adjust base pose accordingly xpos = self.robots[0].robot_model.base_xpos_offset["table"]( self.table_full_size[0]) self.robots[0].robot_model.set_base_xpos(xpos) # load model for table top workspace self.mujoco_arena = TableArena( table_full_size=self.table_full_size, table_friction=self.table_friction, table_offset=(0, 0, 0.8), ) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # Arena always gets set to zero origin self.mujoco_arena.set_origin([0, 0, 0]) # initialize objects of interest tex_attrib = { "type": "cube", } mat_attrib = { "texrepeat": "1 1", "specular": "0.4", "shininess": "0.1", } redwood = CustomMaterial( texture="WoodRed", tex_name="redwood", mat_name="redwood_mat", tex_attrib=tex_attrib, mat_attrib=mat_attrib, ) greenwood = CustomMaterial( texture="WoodGreen", tex_name="greenwood", mat_name="greenwood_mat", tex_attrib=tex_attrib, mat_attrib=mat_attrib, ) cubeA = BoxObject( name="cubeA", size_min=[0.02, 0.02, 0.02], size_max=[0.02, 0.02, 0.02], rgba=[1, 0, 0, 1], material=redwood, ) cubeB = BoxObject( name="cubeB", size_min=[0.025, 0.025, 0.025], size_max=[0.025, 0.025, 0.025], rgba=[0, 1, 0, 1], material=greenwood, ) self.mujoco_objects = OrderedDict([("cubeA", cubeA), ("cubeB", cubeB)]) # task includes arena, robot, and objects of interest self.model = ManipulationTask( self.mujoco_arena, [robot.robot_model for robot in self.robots], self.mujoco_objects, initializer=self.placement_initializer, ) self.model.place_objects()
def _load_model(self): """ Loads an xml model, puts it in self.model. This sets up the mujoco xml for the scene. """ super()._load_model() self.mujoco_robot.set_base_xpos([0, 0, 0]) ### Domain Randomisation ### if (self.initialised): for key, val in self._parameter_for_randomisation_generator(parameters=self.parameters_to_randomise): self.dynamics_parameters[key] = val ## Queues for adding time delays self.eef_pos_queue = deque(maxlen=int(self.dynamics_parameters['eef_timedelay'] + 1)) self.eef_vel_queue = deque(maxlen=int(self.dynamics_parameters['eef_timedelay'] + 1)) if (self.pid is not None): self.pid.sample_time = self.dynamics_parameters['pid_iteration_time'] ### Create the Task ### ## Load the Arena ## self.mujoco_arena = TableArena( table_full_size=self.table_full_size, table_friction=(0.1, 0.001, 0.001) ) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # The sawyer robot has a pedestal, we want to align it with the table self.mujoco_arena.set_origin([0.16 + self.table_full_size[0] / 2, 0, 0]) goal = BoxObject(size=[0.023 / 2, 0.023 / 2, 0.001 / 2], rgba=[0, 1, 0, 1], ) ## Put everything together into the task ## self.model = ReachTask(self.mujoco_arena, self.mujoco_robot, goal) ### Set the goal position ### # Gripper position at neutral gripper_pos_neutral = [0.44969246, 0.16029991, 1.00875409] if(self.specific_goal_position is not None): init_pos = np.array([gripper_pos_neutral[0] +self.specific_goal_position[0], gripper_pos_neutral[1] + self.specific_goal_position[1], self.model.table_top_offset[2]]) init_pos = array_to_string(init_pos) elif (self.randomise_initial_conditions): # Place goal in a 20x20cm square directly below the eef neutral position. noise = self.np_random.uniform(-1, 1, 3) * np.array([0.20, 0.20, 0.0]) offset = np.array([gripper_pos_neutral[0], gripper_pos_neutral[1], self.model.table_top_offset[2]]) init_pos = array_to_string(noise + offset) else: init_pos = np.concatenate([gripper_pos_neutral[:2], [self.model.table_top_offset[2]]]) init_pos = array_to_string(init_pos) self.model.xml_goal.set("pos", init_pos) ### Set the xml parameters to the values given by the dynamics_parameters attribute ### if (self.initialised): self._apply_xml_dynamics_parameters()
axis="0 0 1", damping="50")) # Add the dummy body with the joint to the global worldbody world.worldbody.append(gripper_body) # 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",
gripper_body.append( new_joint(name="gripper_z_joint", type="slide", axis="0 0 1", damping="50")) world.merge(gripper, merge_body=False) world.worldbody.append(gripper_body) 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(size=[0.02, 0.02, 0.02], rgba=[1, 0, 0, 1], friction=1).get_collision() mujoco_object.append(new_joint(name="object_free_joint", type="free")) mujoco_object.set("pos", "0 0 0.11") geoms = mujoco_object.findall("./geom") for geom in geoms: if geom.get("contype"): pass geom.set("name", "object") geom.set("density", "10000") # 1000 for water world.worldbody.append(mujoco_object) x_ref = BoxObject(size=[0.01, 0.01, 0.01], rgba=[0, 1, 0, 1]).get_visual() x_ref.set("pos", "0.2 0 0.105") world.worldbody.append(x_ref) y_ref = BoxObject(size=[0.01, 0.01, 0.01], rgba=[0, 0, 1, 1]).get_visual()