def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = np.array([ -0.498, 0.005, -0.431 + self.gripper_extra_height ]) + self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) gripper_target = np.array([ -0.798, -0.25, -0.331 + self.gripper_extra_height ]) + self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([0., 0., 0., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos( 'robot0:grip').copy() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _build_reach_helper_test_robot(max_position_change=0.020): from gym.envs.robotics import utils from robogym.envs.rearrange.simulation.blocks import ( BlockRearrangeSim, BlockRearrangeSimParameters, ) from robogym.robot.robot_interface import ControlMode, RobotControlParameters sim = BlockRearrangeSim.build( n_substeps=20, robot_control_params=RobotControlParameters( control_mode=ControlMode.TCP_WRIST.value, max_position_change=max_position_change, ), simulation_params=BlockRearrangeSimParameters(), ) # reset mocap welds if any. This is actually needed for TCP arms to move utils.reset_mocap_welds(sim.mj_sim) # extract arm since CompositeRobots are not fully supported by reach_helper composite_robot = sim.robot arm = composite_robot.robots[0] arm.autostep = True return arm
def _env_setup(self, initial_qpos): self.id2obj = [self._geom2objid(i) for i in range(self.sim.model.ngeom)] for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. if not hasattr(self, 'initial_gripper_xpos'): self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy() self.height_offset = self.sim.data.get_site_xpos('object0')[2] # Change the colors to match the success conditions self.color_modder = TextureModder(self.sim) for i in range(self.sim.model.ngeom): obj_id = self.id2obj[i] if obj_id != None: name = self.sim.model.geom_id2name(i) if 'table' in name: color = np.asarray(COLORS_RGB[self.obj_colors[obj_id]]) self.color_modder.set_rgb(name, color * 2) else: self.color_modder.set_rgb(name, COLORS_RGB[self.obj_colors[obj_id]])
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) if self.cam_type != "fixed": delta_pos = self.np_random.uniform(-0.03, 0.03, size=3) self.cam_offset = delta_pos # delta_rot = self.np_random.uniform(-0.1, 0.1, size=3) delta_rot = np.array([0, 0, 0]) utils.cam_init_pos(self.sim, delta_pos, delta_rot) self.sim.forward() # Move end effector into position. if self.gripper_init_type != "fixed": init_disturbance = np.array([ self.np_random.uniform(-0.05, 0.05), self.np_random.uniform(-0.05, 0.05), 0.2 ]) else: init_disturbance = np.array([0, 0, 0.2]) gripper_target = np.array([ -0.498, 0.005, -0.431 + self.gripper_extra_height ]) + init_disturbance + self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos( 'robot0:grip').copy() self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos, initial_rot): ''' Render goals here acording to the basic motion ''' print('************* Initial env setup *************') for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip') print('Set initial position of the endefector = ', gripper_target) gripper_rotation = initial_rot self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) self._sample_goals() assert len(self.goals) >0 for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
def _env_setup(self, initial_qpos): """ TODO: We can initialize the end effector using this method. """ for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) gym_robotics_utils.reset_mocap_welds(self.sim) self.sim.forward() if self.easy_init: ### new position, closer to hook at start ### gripper_target = np.array([-0.7, -0.4, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip') else: ### the old, initial position ### gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # compute Middle point # TODO: initial markers (index 3 nur zufällig, aufpassen!) self.middle_point = self.sim.data.get_site_xpos('middle_point') self.middle_point[2] += 0.2 # adapt height sites_offset = (self.sim.data.site_xpos - self.sim.model.site_pos).copy()[3] # Move end effector into position. # TODO: changed that to the left #gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip') gripper_target = self.middle_point + self.gripper_extra_height #+ self.sim.data.get_site_xpos('robot0:grip') gripper_target[0] -= self.target_range_x gripper_target[1] += self.target_range_y gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy() object_xpos = self.initial_gripper_xpos object_xpos[2] = 0.4 # table height site_id = self.sim.model.site_name2id('init_1') self.sim.model.site_pos[site_id] = object_xpos + [self.obj_range, self.obj_range, 0.0] - sites_offset site_id = self.sim.model.site_name2id('init_2') self.sim.model.site_pos[site_id] = object_xpos + [self.obj_range, -self.obj_range, 0.0] - sites_offset site_id = self.sim.model.site_name2id('init_3') self.sim.model.site_pos[site_id] = object_xpos + [-self.obj_range, self.obj_range, 0.0] - sites_offset site_id = self.sim.model.site_name2id('init_4') self.sim.model.site_pos[site_id] = object_xpos + [-self.obj_range, -self.obj_range, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark1') self.sim.model.site_pos[site_id] = self.middle_point + [-2*self.target_range_x, 0.0, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark2') self.sim.model.site_pos[site_id] = self.middle_point + [2*self.target_range_x, 0.0, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark3') self.sim.model.site_pos[site_id] = self.middle_point + [0.0, 2*self.target_range_y, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark4') self.sim.model.site_pos[site_id] = self.middle_point + [0.0, -2*self.target_range_y, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark5') self.sim.model.site_pos[site_id] = self.middle_point + [2*self.target_range_x, 2*self.target_range_y, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark6') self.sim.model.site_pos[site_id] = self.middle_point + [-2*self.target_range_x, 2*self.target_range_y, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark7') self.sim.model.site_pos[site_id] = self.middle_point + [2*self.target_range_x, -2*self.target_range_y, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark8') self.sim.model.site_pos[site_id] = self.middle_point + [-2*self.target_range_x, -2*self.target_range_y, 0.0] - sites_offset self.sim.step() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # initial markers (index 3 is arbitrary) self.target_1 = self.sim.data.get_site_xpos('target_1') self.target_2 = self.sim.data.get_site_xpos('target_2') self.target_3 = self.sim.data.get_site_xpos('target_3') self.target_4 = self.sim.data.get_site_xpos('target_4') self.target_5 = self.sim.data.get_site_xpos('target_5') self.target_6 = self.sim.data.get_site_xpos('target_6') self.target_7 = self.sim.data.get_site_xpos('target_7') self.target_8 = self.sim.data.get_site_xpos('target_8') self.targets = [self.target_1, self.target_2, self.target_3, self.target_4, self.target_5, self.target_6, self.target_7, self.target_8] self.init_center = self.sim.data.get_site_xpos('init_center') sites_offset = (self.sim.data.site_xpos - self.sim.model.site_pos).copy()[3] # Move end effector into position. gripper_target = self.init_center + self.gripper_extra_height #+ self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy() object_xpos = self.initial_gripper_xpos object_xpos[2] = 0.4 # table height site_id = self.sim.model.site_name2id('init_1') self.sim.model.site_pos[site_id] = object_xpos + [self.obj_range, self.obj_range, 0.0] - sites_offset site_id = self.sim.model.site_name2id('init_2') self.sim.model.site_pos[site_id] = object_xpos + [self.obj_range, -self.obj_range, 0.0] - sites_offset site_id = self.sim.model.site_name2id('init_3') self.sim.model.site_pos[site_id] = object_xpos + [-self.obj_range, self.obj_range, 0.0] - sites_offset site_id = self.sim.model.site_name2id('init_4') self.sim.model.site_pos[site_id] = object_xpos + [-self.obj_range, -self.obj_range, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark1') self.sim.model.site_pos[site_id] = self.target_1 + [self.target_range_x, self.target_range_y, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark2') self.sim.model.site_pos[site_id] = self.target_1 + [-self.target_range_x, self.target_range_y, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark3') self.sim.model.site_pos[site_id] = self.target_1 + [self.target_range_x, -self.target_range_y, 0.0] - sites_offset site_id = self.sim.model.site_name2id('mark4') self.sim.model.site_pos[site_id] = self.target_1 + [-self.target_range_x, -self.target_range_y, 0.0] - sites_offset self.sim.step() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos( 'robot0:grip').copy()
def dobot_env_setup(sim): initial_qpos = { 'robot0:slide0': 0.0, 'robot0:slide1': 0.0, 'robot0:slide2': 0.0, } for name, value in initial_qpos.items(): sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(sim) sim.forward() for _ in range(10): sim.step()
def reset(self): for name, value in self.initial_qpos.items(): self.data.set_joint_qpos(name, value) reset_mocap_welds(self.sim) self.sim.forward() # not sure if this is required. therefore not using currently # self.sim.reset() ob = self.reset_model() if self.viewer is not None: self.viewer_setup() return ob
def _env_setup(self, initial_qpos): if initial_qpos is not None: raise NotImplementedError reset_mocap_welds(self.sim) self.sim.forward() self.init_qpos = self.sim.data.qpos.ravel().copy() self.init_qvel = self.sim.data.qvel.ravel().copy() yumi_arm_joints = [1, 2, 7, 3, 4, 5, 6] if self.has_right_arm: self._arm_r_joint_idx = [ self.sim.model.joint_name2id(f'yumi_joint_{i}_r') for i in yumi_arm_joints ] self.arm_r_joint_lims = self.sim.model.jnt_range[ self._arm_r_joint_idx].copy() if self.has_left_arm: self._arm_l_joint_idx = [ self.sim.model.joint_name2id(f'yumi_joint_{i}_l') for i in yumi_arm_joints ] self.arm_l_joint_lims = self.sim.model.jnt_range[ self._arm_l_joint_idx].copy() if not self.block_gripper: if self.has_right_arm: self._gripper_r_joint_idx = [ self.sim.model.joint_name2id('gripper_r_joint'), self.sim.model.joint_name2id('gripper_r_joint_m') ] if self.has_left_arm: self._gripper_l_joint_idx = [ self.sim.model.joint_name2id('gripper_l_joint'), self.sim.model.joint_name2id('gripper_l_joint_m') ] # Extract information for sampling goals. if self.has_left_arm: self._initial_l_gripper_pos = self.sim.data.get_site_xpos( 'gripper_l_center').copy() if self.has_right_arm: self._initial_r_gripper_pos = self.sim.data.get_site_xpos( 'gripper_r_center').copy() if self.has_object: for _ in range(10): self.sim.step() self._object_z_offset = self.sim.data.get_body_xpos('object0')[2] self._reset_sim()
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. # gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('r_grip_site') gripper_target = np.array([ 0.498, 0.005, 0.431 + self.gripper_extra_height ]) + self.sim.data.get_site_xpos('r_grip_site') if self.debug_print: print("gripper quat: ", self.sim.data.get_site_xmat('r_grip_site')) print("get_mocap_quat: ", self.sim.data.get_mocap_quat('gripper_r:mocap')) # gripper_rotation = np.array([1., 0., 1., 0.]) # gripper_rotation = np.array([-0.82031777, -0.33347336, -0.32553968, 0.33150896]) # gripper_target = np.array([0.9, -0.3, 0.6]) gripper_target = np.array([0.5, -0.3, 0.6]) # gripper_rotation = np.array([0.5, -0.5, -0.5, -0.5]) #(-90, 0, -90) # gripper_rotation = np.array([0.5, 0.5, 0.5, -0.5]) #(90, 0, 90) gripper down # gripper_rotation = np.array([0.707, 0.0, 0.0, -0.707]) #(0, 0, -90) gripper_rotation = np.array([0, 0.707, 0.707, 0]) #(0, 0, -90) # gripper_rotation = np.array([1.0, 0, 0, 0]) # set random gripper position # for i in range(3): # gripper_target[i] += self.np_random.uniform(-0.2, 0.2) # print("gripper target random: ", gripper_target) self.sim.data.set_mocap_pos('gripper_r:mocap', gripper_target) self.sim.data.set_mocap_quat('gripper_r:mocap', gripper_rotation) for _ in range(10): self.sim.step() # # set random end-effector position # end_effector_pos = self.initial_gripper_xpos # rot_ctrl = [0, 0.707, 0.707, 0] #(0 0 0) # end_effector_pos = np.concatenate([end_effector_pos, rot_ctrl]) # print("end_effector_pos: ", end_effector_pos) # for i in range(3): # end_effector_pos[i] = self.initial_gripper_xpos[i] + self.np_random.uniform(-0.1, 0.1) # utils.mocap_set_action(self.sim, end_effector_pos) # arm control in cartesion (x, y, z) # # for _ in range(100): # # self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos( 'r_grip_site').copy() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. self._set_arm_pose(self._initial_arm_mocap_pose.copy()) for _ in range(10): self.sim.step() if self.has_object: self.height_offset = self.sim.data.get_site_xpos( 'object:center')[2]
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. self.initial_pusher_xpos = self.sim.data.get_site_xpos('pusher').copy() pusher_target = self.initial_pusher_xpos[:3] pusher_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('ur5_mocap', pusher_target) self.sim.data.set_mocap_quat('ur5_mocap', pusher_rotation) for _ in range(10): self.sim.step()
def _build_robot(control_mode, max_position_change): sim = BlockRearrangeSim.build( n_substeps=1, robot_control_params=RobotControlParameters( control_mode=control_mode, max_position_change=max_position_change, ), simulation_params=BlockRearrangeSimParameters(), ) # reset mocap welds if any. This is actually needed for TCP arms to move, so other tests that did not use # env may benefit from it. utils.reset_mocap_welds(sim.mj_sim) return sim.robot
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. self._set_gripper_during_setup() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos( 'robot0:grip').copy() if self.has_object: #self.height_offset = self.sim.data.get_site_xpos('object0')[2] #todo self.height_offset = 0.425
def _initialize_sim_state(self): if self.parameters.robot_control_params.is_joint_actuated(): for idx, eq_type in enumerate(self.mujoco_simulation.mj_sim.model.eq_type): if eq_type == MujocoEquality.mjEQ_WELD.value: self.mujoco_simulation.mj_sim.model.eq_active[idx] = 0 else: utils.reset_mocap_welds(self.sim) self.sim.forward() tcp_pos = self.mujoco_simulation.mj_sim.data.get_body_xpos( "robot0:gripper_base" ) tcp_quat = self.sim.data.get_body_xquat("robot0:gripper_base") self.mujoco_simulation.mj_sim.data.set_mocap_pos("robot0:mocap", tcp_pos) self.mujoco_simulation.mj_sim.data.set_mocap_quat("robot0:mocap", tcp_quat) for _ in range(10): self.mujoco_simulation.step() self.robot.reset()
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = np.array([0.6, 0.6 , 0.4 + self.gripper_extra_height]) #+ self.sim.data.get_site_xpos('robotiq_85_base_link') gripper_rotation = np.array([0., 1., 1., 0.]) self.sim.data.set_mocap_pos('robot1:mocap', gripper_target) self.sim.data.set_mocap_quat('robot1:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_body_xpos('robot1:ee_link').copy() # Needs a change if using the gripper for goal generation if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) robot_utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = np.array([ -0.498, 0.005, -0.431 + self.gripper_extra_height ]) + self.sim.data.get_site_xpos("robot0:grip") gripper_rotation = np.array([1.0, 0.0, 1.0, 0.0]) self.sim.data.set_mocap_pos("robot0:mocap", gripper_target) self.sim.data.set_mocap_quat("robot0:mocap", gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos( "robot0:grip").copy()
def _env_setup(self, initial_qpos): # TODO: initial state of joints for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = self.sim.data.get_body_xpos('robot:left_outer_finger').copy() #+ np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot:mocap', gripper_target) self.sim.data.set_mocap_quat('robot:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_body_xpos('robot:left_outer_finger').copy()
def _env_setup(self): utils.reset_mocap_welds(self.sim) utils.reset_mocap2body_xpos(self.sim) lim1_id = self.sim.model.site_name2id('limit0') lim2_id = self.sim.model.site_name2id('limit1') lim3_id = self.sim.model.site_name2id('limit2') lim4_id = self.sim.model.site_name2id('limit3') self.sim.model.site_pos[lim1_id] = self.origin + \ np.array([-self.maxdist, -self.maxdist, 0]) self.sim.model.site_pos[lim2_id] = self.origin + \ np.array([self.maxdist, -self.maxdist, 0]) self.sim.model.site_pos[lim3_id] = self.origin + \ np.array([-self.maxdist, self.maxdist, 0]) self.sim.model.site_pos[lim4_id] = self.origin + \ np.array([self.maxdist, self.maxdist, 0]) for _ in range(10): self._take_substeps()
def _env_setup(self, initial_qpos): if self.mode == "robot": #self.reset_publisher.publish(String("RESET")) #self.get_observation_command_publisher.publish(String("GET_OBS")) #self.current_observation = np.array(rospy.wait_for_message( # "/pyrobot/observation", numpy_msg(Floats)).data) #eff_pos = self.current_observation[:3] #self.initial_gripper_xpos = eff_pos # DEBUG: #rospy.loginfo('[DEBUG] env setup initial gripper xpos = {}'.format(self.initial_gripper_xpos)) pass elif self.mode == "sim": # Setup the initial joint positions for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) self.initial_state_2 = self.sim.get_state() utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into positions | -0.498, 0.005, -0.431 # + self.gripper_extra_height # DEBUG: #print('[ENV SETUP][BEFORE] initial_gripper_xpos = {}'.format(self.sim.data.get_site_xpos('robot0:end_effector'))) #gripper_target = np.array([-0.498, 0.005, -0.431+ self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:end_effector') #gripper_target = np.array([-0.298, 0.005, -0.431])+ self.sim.data.get_site_xpos('robot0:end_effector') #gripper_target = self.sim.data.get_site_xpos('robot0:end_effector') #gripper_target = np.array([1.15360479, 0.74419554, 0.40428726]) #gripper_target = np.array([1.10412612, 0.744193, 0.17162448]) #gripper_rotation = np.array([1., 0., 1., 0.]) #gripper_rotation = np.array([0., 0., 0., 1.]) #print('[BEFORE] sim.data.mocap_pos = {}'.format(self.sim.data.mocap_pos)) #print('gripper_target = {}'.format(gripper_target)) #self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) #self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) #print('[After] sim.data.mocap_pos = {}'.format(self.sim.data.mocap_pos)) #self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:end_effector').copy() #print('[ENV SETUP][AFTER] initial_gripper_xpos = {}'.format(self.initial_gripper_xpos)) #for _ in range(10): # self.sim.step() self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:end_effector').copy() print('[ENV SETUP][AFTER] initial_gripper_xpos = {}'.format(self.initial_gripper_xpos)) print('[ENV SETUP][AFTER] sim.data.mocap_pos = {}'.format(self.sim.data.mocap_pos))
def _env_setup(self, initial_qpos): for name in initial_qpos: self.sim.data.set_joint_qpos(name, initial_qpos[name]) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move gripper into position gripper_target = np.array( [0.0, 0.0, 0.0]) + self.sim.data.get_site_xpos('robot0:gripper') gripper_rotation = np.array([1., 0., 0., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals self.initial_gripper_xpos = self.sim.data.get_site_xpos( 'robot0:gripper').copy() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _reset_sim(self): RobotEnv._reset_sim(self) # Move end-effector into position gripper_rotation = np.array([1., 0., 1., 0.]) if self.random_initial_gripper_position: # Limits of actuation: right on the back edge to just before the front edge x = np.random.uniform(1.05, 1.5) # A little beyond the left edge to a little beyond the right edge y = np.random.uniform(0.3, 1.2) # Right on the table to medium-high z = np.random.uniform(0.45, 0.8) gripper_target = np.array([x, y, z]) else: # Above middle of table gripper_target = [1.3419, 0.7491, 0.755] self.sim.data.set_mocap_pos('mocap', gripper_target) self.sim.data.set_mocap_quat('mocap', gripper_rotation) self.sim.model.eq_active[0] = 1 # Enable mocap # Not sure what this does, but FetchEnv has it utils.reset_mocap_welds(self.sim) self.sim.forward() for _ in range(10): self.sim.step() # The actuators will try to fight against the mocap control. # From https://github.com/openai/gym/issues/234, it seems like we can't # turn off the actuators (by adjusting e.g. actuator_gainprm). # So instead, we just update the actuators to be closer to where the # mocap is trying to pull them. for n, actuator_name in enumerate(self.sim.model.actuator_names): pos = self.sim.data.get_joint_qpos(actuator_name) self.sim.data.ctrl[n] = pos # Disable mocap and give gripper time to stop bouncing self.sim.model.eq_active[0] = 0 for _ in range(10): self.sim.step() return True
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. Fetch:-0.498, 0.005, -0.431 baxter:-0.151, -0.831, -0.602 # gripper_target = np.array([0.151, 0.431, 0 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip') #---baxter # gripper_target = np.array([0.5824, 0.1861, 0.1207]) #---baxter # gripper_rotation = np.array([1., 0., 1., 0.]) #---baxter [0 0 1 0] # self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) # self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) # for _ in range(10): # ------------baxter # self.sim.step() # ------------baxter # self.sim.step() # ------------baxter # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos( 'robot0:grip').copy() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): # qpos = self.sim.data.get_joint_qpos(name) # print(f"{name}: {qpos}") self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) \ + self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy() self.initial_heights = {} for obj_key in self.obj_keys: self.initial_heights[obj_key] = self.sim.data.get_site_xpos(obj_key)[2]
def _env_setup(self, initial_qpos): # TODO: initial state of joints for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = self.sim.data.get_body_xpos(self.finder_name).copy( ) #+ np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) # gripper_rotation is 1 0 1 0 for wirst_1_link as finder_name gripper_rotation = np.array([0., 0., 1., 0.]) self.sim.data.set_mocap_pos(self.mocap_name, gripper_target) self.sim.data.set_mocap_quat(self.mocap_name, gripper_rotation) # move gripper into position for i in range(3): self.sim.data.ctrl[i] = 0.8 for _ in range(10): self.sim.step() # move object into position ref_pos = self.sim.data.get_body_xpos(self.finder_name) offset = np.array([0., 0., -0.12]) pos = ref_pos + offset rot = [0, 0, 0, 0] ob_pos = list(pos) + rot self.sim.data.set_joint_qpos("object0:joint", ob_pos) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_object_xpos = self.sim.data.get_body_xpos( self.object_name).copy() self.initial_gripper_xpos = self.sim.data.get_body_xpos( self.finder_name).copy()
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. self.sim.data.set_mocap_quat("robot0:mocap", np.array([1.0, 0.0, 1.0, 0.0])) gripper_target = np.array([ -0.59, 0.0, -0.3 + self.gripper_extra_height ]) + self.sim.data.get_site_xpos("robot0:grip") self._env_setup_helper(gripper_target, 1.0) gripper_target = np.array( [0.0, 0.0, -0.07]) + self.sim.data.get_site_xpos("robot0:grip") self._env_setup_helper(gripper_target, 1.0) gripper_target = np.array( [0.0, 0.0, 0.0]) + self.sim.data.get_site_xpos("robot0:grip") self._env_setup_helper(gripper_target, -1.0) gripper_target = np.array( [0.0, 0.0, -0.05]) + self.sim.data.get_site_xpos("robot0:grip") self._env_setup_helper(gripper_target, -1.0) # Extract information for sampling goals. self.goal = self.sim.data.get_site_xpos("hole").copy() + np.array( (0.0, 0.0, 0.0))
def _env_setup(self, initial_qpos): for name, value in initial_qpos.items(): self.sim.data.set_joint_qpos(name, value) utils.reset_mocap_welds(self.sim) self.sim.forward() # Move end effector into position. gripper_target = np.array([ -0.498, 0.005, -0.431 + self.gripper_extra_height ]) + self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() # Extract information for sampling goals. self.initial_gripper_xpos = self.sim.data.get_site_xpos( 'robot0:grip').copy() # TODO: initial markers (index 2 nur zufällig, aufpassen!) object_xpos = self.initial_gripper_xpos object_xpos[2] = 0.4 sites_offset = (self.sim.data.site_xpos - self.sim.model.site_pos).copy()[2] site_id = self.sim.model.site_name2id('init_1') self.sim.model.site_pos[site_id] = object_xpos + [ self.obj_range, self.obj_range, 0.0 ] - sites_offset site_id = self.sim.model.site_name2id('init_2') self.sim.model.site_pos[site_id] = object_xpos + [ self.obj_range, -self.obj_range, 0.0 ] - sites_offset site_id = self.sim.model.site_name2id('init_3') self.sim.model.site_pos[site_id] = object_xpos + [ -self.obj_range, self.obj_range, 0.0 ] - sites_offset site_id = self.sim.model.site_name2id('init_4') self.sim.model.site_pos[site_id] = object_xpos + [ -self.obj_range, -self.obj_range, 0.0 ] - sites_offset site_id = self.sim.model.site_name2id('init_1a') self.sim.model.site_pos[site_id] = object_xpos + [ self.obj_range, self.obj_range, self.air_height / 2 ] - sites_offset site_id = self.sim.model.site_name2id('init_2a') self.sim.model.site_pos[site_id] = object_xpos + [ self.obj_range, -self.obj_range, self.air_height / 2 ] - sites_offset site_id = self.sim.model.site_name2id('init_3a') self.sim.model.site_pos[site_id] = object_xpos + [ -self.obj_range, self.obj_range, self.air_height / 2 ] - sites_offset site_id = self.sim.model.site_name2id('init_4a') self.sim.model.site_pos[site_id] = object_xpos + [ -self.obj_range, -self.obj_range, self.air_height / 2 ] - sites_offset site_id = self.sim.model.site_name2id('init_1b') self.sim.model.site_pos[site_id] = object_xpos + [ self.obj_range, self.obj_range, self.air_height ] - sites_offset site_id = self.sim.model.site_name2id('init_2b') self.sim.model.site_pos[site_id] = object_xpos + [ self.obj_range, -self.obj_range, self.air_height ] - sites_offset site_id = self.sim.model.site_name2id('init_3b') self.sim.model.site_pos[site_id] = object_xpos + [ -self.obj_range, self.obj_range, self.air_height ] - sites_offset site_id = self.sim.model.site_name2id('init_4b') self.sim.model.site_pos[site_id] = object_xpos + [ -self.obj_range, -self.obj_range, self.air_height ] - sites_offset site_id = self.sim.model.site_name2id('mark0a') self.sim.model.site_pos[site_id] = object_xpos + [ self.target_range, self.target_range, 0.0 ] - sites_offset site_id = self.sim.model.site_name2id('mark1a') self.sim.model.site_pos[site_id] = object_xpos + [ self.target_range, -self.target_range, 0.0 ] - sites_offset site_id = self.sim.model.site_name2id('mark2a') self.sim.model.site_pos[site_id] = object_xpos + [ -self.target_range, self.target_range, 0.0 ] - sites_offset site_id = self.sim.model.site_name2id('mark3a') self.sim.model.site_pos[site_id] = object_xpos + [ -self.target_range, -self.target_range, 0.0 ] - sites_offset site_id = self.sim.model.site_name2id('mark0b') self.sim.model.site_pos[site_id] = object_xpos + [ self.target_range, self.target_range, self.air_height / 2 ] - sites_offset site_id = self.sim.model.site_name2id('mark1b') self.sim.model.site_pos[site_id] = object_xpos + [ self.target_range, -self.target_range, self.air_height / 2 ] - sites_offset site_id = self.sim.model.site_name2id('mark2b') self.sim.model.site_pos[site_id] = object_xpos + [ -self.target_range, self.target_range, self.air_height / 2 ] - sites_offset site_id = self.sim.model.site_name2id('mark3b') self.sim.model.site_pos[site_id] = object_xpos + [ -self.target_range, -self.target_range, self.air_height / 2 ] - sites_offset site_id = self.sim.model.site_name2id('mark0c') self.sim.model.site_pos[site_id] = object_xpos + [ self.target_range, self.target_range, self.air_height ] - sites_offset site_id = self.sim.model.site_name2id('mark1c') self.sim.model.site_pos[site_id] = object_xpos + [ self.target_range, -self.target_range, self.air_height ] - sites_offset site_id = self.sim.model.site_name2id('mark2c') self.sim.model.site_pos[site_id] = object_xpos + [ -self.target_range, self.target_range, self.air_height ] - sites_offset site_id = self.sim.model.site_name2id('mark3c') self.sim.model.site_pos[site_id] = object_xpos + [ -self.target_range, -self.target_range, self.air_height ] - sites_offset self.sim.step() if self.has_object: self.height_offset = self.sim.data.get_site_xpos('object0')[2]