def init(self): b = self.scene.create_actor_builder() b.add_box_shape(size=[0.02, .5, 1]) b.add_box_visual(size=[0.02, .5, 1]) w1 = b.build(True) w1.set_pose(Pose([-0.2, 0, 1])) b = self.scene.create_actor_builder() b.add_box_shape(size=[0.02, .5, 1]) b.add_box_visual(size=[0.02, .5, 1]) w2 = b.build(True) w2.set_pose(Pose([0.2, 0, 1])) b = self.scene.create_actor_builder() b.add_box_shape(size=[0.25, 0.02, 1]) b.add_box_visual(size=[0.25, 0.02, 1]) w3 = b.build(True) w3.set_pose(Pose([0, -0.4, 1])) b = self.scene.create_actor_builder() b.add_box_shape(size=[0.25, 0.02, 1]) b.add_box_visual(size=[0.25, 0.02, 1]) w4 = b.build(True) w4.set_pose(Pose([0, 0.4, 1])) for i in range(1500): self.scene.step() self.scene.remove_actor(w1) self.scene.remove_actor(w2) self.scene.remove_actor(w3) self.scene.remove_actor(w4)
def reset(self): self.total_box_genreated += 10 self.local_total_timesteps = 0 if self.left_robot: self.scene.remove_articulation(self.left_robot.robot) self.scene.remove_articulation(self.right_robot.robot) base_size = np.array([0.015, 0.07, 0.07]) size = base_size + np.random.random(3) * np.array([0.01, 0.06, 0.06]) self.left_robot = Robot( self.load_robot(Pose([-.5, .25, 0.6], axangle2quat([0, 0, 1], -np.pi / 2)), size)) self.left_robot.robot.set_qpos([1.5, 1, 0, -1, 0, 0, 0]) self.right_robot = Robot( self.load_robot(Pose([-.5, -.25, 0.6], axangle2quat([0, 0, 1], np.pi / 2)), size)) self.right_robot.robot.set_qpos([-1.5, 1, 0, -1, 0, 0, 0]) self.create_bin([-1.2 + np.random.random() * 0.1, np.random.random() * 0.2 - 0.1, 0.6], np.random.random() * np.pi, [ 0.2 + np.random.random() * 0.05, 0.3 + np.random.random() * 0.05, 0.4 + np.random.random() * 0.05 ], 0.015 + np.random.random() * 0.05) for d in self.boxes: d.set_pose(Pose([np.random.random() * 0.2 - 0.1, np.random.random() * 0.4 - 0.2, 2])) self.init() self.scene.update_render()
def setup_boxes(self): object_material = sim.create_physical_material(1, 1, 0.1) size = 0.02 b = self.scene.create_actor_builder() b.add_box_shape(size=[size] * 3, material=object_material) b.add_box_visual(size=[size] * 3, color=[1, 0, 0]) red_box = b.build() red_box.set_pose(Pose([0.4, 0, size])) red_box.set_name('red') b = self.scene.create_actor_builder() b.add_box_shape(size=[size] * 3, material=object_material) b.add_box_visual(size=[size] * 3, color=[0, 1, 0]) green_box = b.build() green_box.set_pose(Pose([0.4, 0.2, size])) green_box.set_name('green') b = self.scene.create_actor_builder() b.add_box_shape(size=[size] * 3, material=object_material) b.add_box_visual(size=[size] * 3, color=[0, 0, 1]) blue_box = b.build() blue_box.set_pose(Pose([0.4, -0.2, size])) blue_box.set_name('blue') self.boxes = red_box, green_box, blue_box
def __init__(self, frame_skip, timestep=0.01, gravity=(0, 0, -9.8)): self.frame_skip = frame_skip self.timestep = timestep self._engine = sapien.Engine() self._renderer = sapien.VulkanRenderer(True) self._engine.set_renderer(self._renderer) self.sim = self._engine.create_scene() self.sim.set_timestep(timestep) self.viewer_setup() self.model, self._init_state = self._load_model() self._dof = self.model.dof self._root = self.model.get_links()[0] # Unlike MuJoCo MJCF, the actuator information is not defined in the xml file # So you should define it explicitly in Python from gym.spaces.box import Box actuator_info = self._load_actuator() self._actuator_name, _actuator_idx, low, high = list( zip(*actuator_info)) self._actuator_idx = np.array(_actuator_idx) self.action_spec = Box(low=np.array(low), high=np.array(high), dtype=np.float32) near, far = 0.1, 100 camera_mount_actor = self.sim.create_actor_builder().build( is_kinematic=True) camera = self.sim.add_mounted_camera( "first_camera", camera_mount_actor, Pose(), 640, 480, 0, np.deg2rad(35), near, far, ) pos = np.array([-2, -2, 3]) forward = -pos / np.linalg.norm(pos) left = np.cross([0, 0, 1], forward) left = left / np.linalg.norm(left) up = np.cross(forward, left) mat44 = np.eye(4) mat44[:3, :3] = np.array([forward, left, up]).T mat44[:3, 3] = pos camera_mount_actor.set_pose(Pose.from_transformation_matrix(mat44)) self.camera = camera
def load_robot(self, pose, size, thickness=0.003, offset=0.01): x, y, z = size lb = self.robot_builder.get_link_builders() lb[9].remove_all_shapes() lb[9].remove_all_visuals() lb[9].add_box_shape(Pose([-x / 2, 0, z / 2 + offset]), [thickness / 2, y / 2, z / 2]) lb[9].add_box_visual_complex(Pose([-x / 2, 0, z / 2 + offset]), [thickness / 2, y / 2, z / 2], steel) lb[9].add_box_shape(Pose([0, y / 2, z / 2 + offset]), [x / 2 + thickness / 2, thickness / 2, z / 2]) lb[9].add_box_visual_complex( Pose([0, y / 2, z / 2 + offset]), [x / 2 + thickness / 2, thickness / 2, z / 2], steel) lb[9].add_box_shape(Pose([0, -y / 2, z / 2 + offset]), [x / 2 + thickness / 2, thickness / 2, z / 2]) lb[9].add_box_visual_complex( Pose([0, -y / 2, z / 2 + offset]), [x / 2 + thickness / 2, thickness / 2, z / 2], steel) lb[9].add_box_shape( Pose([0, 0, offset]), [x / 2 + thickness / 2, y / 2 + thickness / 2, thickness / 2]) lb[9].add_box_visual_complex(Pose([ 0, 0, offset ]), [x / 2 + thickness / 2, y / 2 + thickness / 2, thickness / 2], steel) r = self.robot_builder.build(True) r.name = "robot" r.set_root_pose(pose) return r
def build_room(self): b = self.scene.create_actor_builder() b.add_box_shape(Pose([0, 0, -0.1]), [2.5, 2.5, 0.1]) b.add_box_shape(Pose([0, -2.5, 1.5]), [2.5, 0.1, 1.5]) b.add_box_shape(Pose([0, 2.5, 1.5]), [2.5, 0.1, 1.5]) b.add_box_shape(Pose([-2.5, 0, 1.5]), [0.1, 2.5, 1.5]) b.add_box_shape(Pose([2.5, 0, 1.5]), [0.1, 2.5, 1.5]) b.add_box_visual(Pose([0, 0, -0.1]), [2.5, 2.5, 0.1], [0.2, 0.2, 0.2]) b.add_box_visual(Pose([0, -2.5, 1.5]), [2.5, 0.1, 1.5], [0.8, 0.8, 0.8]) b.add_box_visual(Pose([0, 2.5, 1.5]), [2.5, 0.1, 1.5], [0.8, 0.8, 0.8]) b.add_box_visual(Pose([-2.5, 0, 1.5]), [0.1, 2.5, 1.5], [0.8, 0.8, 0.8]) b.add_box_visual(Pose([2.5, 0, 1.5]), [0.1, 2.5, 1.5], [0.8, 0.8, 0.8]) self.room = b.build_static(name="room")
def init(self, env: Env) -> None: if self._initialized: print(f"Task: {TASK_NAME} already initialized") return # add box self._box = env.agents[BOX_NAME] assert self._box is not None and isinstance(self._box, Box), \ f"{BOX_NAME} does not exist in env" self._box_height_thresh = self._box.observation['pose'].p[ 2] + BOX_HEIGHT_THRESHOLD # add arm self._arm = env.agents[ARM_NAME] assert self._arm is not None and isinstance(self._arm, PandaArm), \ f"{ARM_NAME} does not exist in env" # build goal self._goal = build_goal(env.scene, self._box.box_size) self._goal.set_pose(Pose(GOAl_POS)) self._box_valid_push = True self._suceeded = False self._tracking = True self._failed_reason = None self._parameters = { "box_size": self._box.box_size, "box_tilt_threshold_rad": TILT_THRESHHOLD, "box_at_goal_threshold_meter": BOX_AT_GOAL_THRESHOLD, "box_height_threshold_global": self._box_height_thresh, "goal_pos": GOAl_POS, } self._initialized = True print(f"Task: {TASK_NAME} initialized")
def init(self, env: Env): if self._initialized: print(f"{self.name} already initialized") return # load arm loader = env.scene.create_urdf_loader() loader.fix_root_link = True arm_path = os.path.join(config.ASSET_DIR, "Arm/panda.urdf") print() if not self._pusher: self._robot = loader.load(arm_path) else: builder = loader.load_file_as_articulation_builder(arm_path) lb = builder.get_link_builders() lb[-1].remove_all_shapes() lb[-1].remove_all_visuals() lb[-2].remove_all_shapes() lb[-2].remove_all_visuals() lb[-3].remove_all_shapes() lb[-3].remove_all_visuals() pusher_size = [0.06, 0.06, 0.03] pusher_pose = Pose([0,0,0.03]) material = env._sim.create_physical_material(0.9, 0.9, 0) lb[-3].add_box_shape(pusher_pose, pusher_size, material=material) lb[-3].add_box_visual_complex(pusher_pose, pusher_size) self._robot = builder.build(True) self.dof = self._robot.dof # adjust damping for joint, d in zip(self._robot.get_joints(), np.array([self._damping] * self._robot.dof)): joint.set_drive_property(stiffness=0, damping=d) self._robot.set_pose(INI_POSE) self._robot.set_qpos(INI_QPOS) self._robot.set_name(self.name) self._active_joints = [j for j in self._robot.get_joints() if j.get_dof() == 1] qpos = np.array([0, 0, 0, -1.5, 0, 1.5, 0.7, 0.4, 0.4]) self._robot.set_qpos(qpos) self._robot.set_qvel([0] * self._robot.dof) self.set_action(qpos, [0] * self._robot.dof, self._robot.compute_passive_force()) # set specs self._action_spec = { "qlimits": self._robot.get_qlimits() } self._observation_spec = None self._initialized = True print(f"CREATED: {self.name} created")
def __init__(self, env, near=0.1, far=100.0, image_size=448, dist=5.0, \ phi=np.pi/5, theta=np.pi, fov=35, random_position=False, fixed_position=False): builder = env.scene.create_actor_builder() camera_mount_actor = builder.build(is_kinematic=True) self.env = env # set camera intrinsics self.camera = env.scene.add_mounted_camera('camera', camera_mount_actor, Pose(), \ image_size, image_size, 0, np.deg2rad(fov), near, far) # set camera extrinsics if random_position: theta = np.random.random() * np.pi*2 phi = (np.random.random()+1) * np.pi/6 if fixed_position: #theta = -np.pi/10 #theta = -np.pi/8 theta = np.pi phi = np.pi/10 pos = np.array([dist*np.cos(phi)*np.cos(theta), \ dist*np.cos(phi)*np.sin(theta), \ dist*np.sin(phi)]) forward = -pos / np.linalg.norm(pos) left = np.cross([0, 0, 1], forward) left = left / np.linalg.norm(left) up = np.cross(forward, left) mat44 = np.eye(4) mat44[:3, :3] = np.vstack([forward, left, up]).T mat44[:3, 3] = pos # mat44 is cam2world mat44[0, 3] += env.object_position_offset self.mat44 = mat44 camera_mount_actor.set_pose(Pose.from_transformation_matrix(mat44)) # log parameters self.near = near self.far = far self.dist = dist self.theta = theta self.phi = phi self.pos = pos
def random_pose_circle(theta_range=None, len=None): if len is None: len = [0.3, 0.6] if theta_range is None: theta_range = [0, 2 * np.pi] theta = np.random.uniform(low=theta_range[0], high=theta_range[1]) r = np.random.uniform(low=len[0], high=len[1]) x = r * np.cos(theta) y = r * np.sin(theta) return Pose(np.array([x, y, -0.99]))
def load_object(self, urdf, material, state='closed'): loader = self.scene.create_urdf_loader() self.object = loader.load(urdf, {"material": material}) #self.object = loader.load(urdf, material) pose = Pose([self.object_position_offset, 0, 0], [1, 0, 0, 0]) self.object.set_root_pose(pose) # compute link actor information self.all_link_ids = [l.get_id() for l in self.object.get_links()] self.movable_link_ids = [] for j in self.object.get_joints(): if j.get_dof() == 1: self.movable_link_ids.append(j.get_child_link().get_id()) if self.flog is not None: self.flog.write('All Actor Link IDs: %s\n' % str(self.all_link_ids)) self.flog.write('All Movable Actor Link IDs: %s\n' % str(self.movable_link_ids)) # set joint property for joint in self.object.get_joints(): joint.set_drive_property(stiffness=0, damping=10) # set initial qpos joint_angles = [] self.joint_angles_lower = [] self.joint_angles_upper = [] for j in self.object.get_joints(): if j.get_dof() == 1: l = process_angle_limit(j.get_limits()[0, 0]) self.joint_angles_lower.append(float(l)) r = process_angle_limit(j.get_limits()[0, 1]) self.joint_angles_upper.append(float(r)) if state == 'closed': joint_angles.append(float(l)) elif state == 'open': joint_angles.append(float(r)) elif state == 'random-middle': joint_angles.append(float(get_random_number(l, r))) elif state == 'random-closed-middle': if np.random.random() < 0.5: joint_angles.append(float(get_random_number(l, r))) else: joint_angles.append(float(l)) else: raise ValueError('ERROR: object init state %s unknown!' % state) self.object.set_qpos(joint_angles) return joint_angles
def plot_figure(up, forward): # cam to world up = mat33 @ up forward = mat33 @ forward up = np.array(up, dtype=np.float32) forward = np.array(forward, dtype=np.float32) left = np.cross(up, forward) left /= np.linalg.norm(left) forward = np.cross(left, up) forward /= np.linalg.norm(forward) rotmat = np.eye(4).astype(np.float32) rotmat[:3, 0] = forward rotmat[:3, 1] = left rotmat[:3, 2] = up rotmat[:3, 3] = position_world - up * 0.1 pose = Pose().from_transformation_matrix(rotmat) robot.robot.set_root_pose(pose) env.render() rgb_final_pose, _, _, _ = cam.get_observation() fimg = Image.fromarray((rgb_final_pose*255).astype(np.uint8)) fimg.save(os.path.join(result_dir, 'action.png'))
def before_step(self, env, task): robot: PandaArm = env.agents[ARM_NAME] box: Box = env.agents[BOX_NAME] if self.phase is Phase.rotate_toward_box: if self.init_control is True: box2base = robot.observation['poses'][0].inv( ) * box.observation['pose'] p = box2base.p theta = np.arctan(p[1] / p[0]) if p[0] < 0 and p[1] < 0: theta -= np.pi elif p[0] < 0 and p[1] > 0: theta += np.pi self.init_control = False self.tg_pose = Pose(robot.observation['poses'][0].p, euler.euler2quat(0, 0, theta)) self.drive_to_pose(robot, self.tg_pose, joint_index=1, theta_thresh=1e-4, theta_abs_thresh=1e-5) if self.drive[robot] is False: self.phase = Phase.move_above_box self.prep_drive(robot) elif self.phase is Phase.move_above_box: if self.init_control is True: self.tg_pose = robot.observation['poses'][-1] self.tg_pose.set_p(box.observation['pose'].p) self.tg_pose = Pose([0, 0, 0.2 + box.box_size]) * self.tg_pose self.init_control = False self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0.6], [0, 0])) if self.drive[robot] is False: self.phase = Phase.tap_down self.prep_drive(robot) elif self.phase is Phase.tap_down: if self.init_control is True: self.tg_pose = robot.observation['poses'][-1] self.tg_pose.set_p(box.observation['pose'].p) self.tg_pose = Pose([0, 0, 0.1 + box.box_size]) * self.tg_pose self.init_control = False self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0.6], [0, 0]), max_v=5e-2) if task.get_trajectory_status()['touched'] is True: self.phase = Phase.tap_up self.prep_drive(robot) elif self.phase is Phase.tap_up: if self.init_control is True: self.tg_pose = robot.observation['poses'][-1] self.tg_pose.set_p(box.observation['pose'].p) self.tg_pose = Pose([0, 0, 0.2 + box.box_size]) * self.tg_pose self.init_control = False self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0.6], [0, 0])) if self.drive[robot] is False: self.phase = Phase.evaluate self.prep_drive(robot) elif self.phase is Phase.evaluate: print(task.get_trajectory_status()) self.phase = Phase.done
def create_ant_builder(scene): builder = scene.create_articulation_builder() body = builder.create_link_builder() body.add_sphere_shape(Pose(), 0.25) body.add_sphere_visual_complex(Pose(), 0.25, copper) body.add_capsule_shape(Pose([0.141, 0, 0]), 0.08, 0.141) body.add_capsule_visual_complex(Pose([0.141, 0, 0]), 0.08, 0.141, copper) body.add_capsule_shape(Pose([-0.141, 0, 0]), 0.08, 0.141) body.add_capsule_visual_complex(Pose([-0.141, 0, 0]), 0.08, 0.141, copper) body.add_capsule_shape(Pose([0, 0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141) body.add_capsule_visual_complex( Pose([0, 0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141, copper) body.add_capsule_shape(Pose([0, -0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141) body.add_capsule_visual_complex( Pose([0, -0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141, copper) body.set_name("body") l1 = builder.create_link_builder(body) l1.set_name("l1") l1.set_joint_name("j1") l1.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]], Pose([0.282, 0, 0], [0.7071068, 0, 0.7071068, 0]), Pose([0.141, 0, 0], [-0.7071068, 0, 0.7071068, 0]), 0.1) l1.add_capsule_shape(Pose(), 0.08, 0.141) l1.add_capsule_visual_complex(Pose(), 0.08, 0.141, copper) l2 = builder.create_link_builder(body) l2.set_name("l2") l2.set_joint_name("j2") l2.set_joint_properties( sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]], Pose([-0.282, 0, 0], [0, -0.7071068, 0, 0.7071068]), Pose([0.141, 0, 0], [-0.7071068, 0, 0.7071068, 0]), 0.1) l2.add_capsule_shape(Pose(), 0.08, 0.141) l2.add_capsule_visual_complex(Pose(), 0.08, 0.141, copper) l3 = builder.create_link_builder(body) l3.set_name("l3") l3.set_joint_name("j3") l3.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]], Pose([0, 0.282, 0], [0.5, -0.5, 0.5, 0.5]), Pose([0.141, 0, 0], [0.7071068, 0, -0.7071068, 0]), 0.1) l3.add_capsule_shape(Pose(), 0.08, 0.141) l3.add_capsule_visual_complex(Pose(), 0.08, 0.141, copper) l4 = builder.create_link_builder(body) l4.set_name("l4") l4.set_joint_name("j4") l4.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]], Pose([0, -0.282, 0], [0.5, 0.5, 0.5, -0.5]), Pose([0.141, 0, 0], [0.7071068, 0, -0.7071068, 0]), 0.1) l4.add_capsule_shape(Pose(), 0.08, 0.141) l4.add_capsule_visual_complex(Pose(), 0.08, 0.141, copper) f1 = builder.create_link_builder(l1) f1.set_name("f1") f1.set_joint_name("j11") f1.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]], Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]), Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1) f1.add_capsule_shape(Pose(), 0.08, 0.282) f1.add_capsule_visual_complex(Pose(), 0.08, 0.282, copper) f2 = builder.create_link_builder(l2) f2.set_name("f2") f2.set_joint_name("j21") f2.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]], Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]), Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1) f2.add_capsule_shape(Pose(), 0.08, 0.282) f2.add_capsule_visual_complex(Pose(), 0.08, 0.282, copper) f3 = builder.create_link_builder(l3) f3.set_name("f3") f3.set_joint_name("j31") f3.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]], Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]), Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1) f3.add_capsule_shape(Pose(), 0.08, 0.282) f3.add_capsule_visual_complex(Pose(), 0.08, 0.282, copper) f4 = builder.create_link_builder(l4) f4.set_name("f4") f4.set_joint_name("j41") f4.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]], Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]), Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1) f4.add_capsule_shape(Pose(), 0.08, 0.282) f4.add_capsule_visual_complex(Pose(), 0.08, 0.282, copper) return builder
return builder render_controller.show_window() s0 = sim.create_scene() s0.add_ground(-3) s0.set_timestep(1 / 240) s0.set_ambient_light([0.5, 0.5, 0.5]) s0.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5]) ant_builder = create_ant_builder(s0) ant = ant_builder.build() ant.set_root_pose(Pose([0, 0, 2])) render_controller.set_camera_position(-5, 0, 2) render_controller.set_current_scene(s0) render_controller.focus(ant.get_links()[0]) for j in ant.get_joints(): j.set_friction(0) s0.step() ant.set_qpos([0, 0, 0, 0, 0.7, 0.7, 0.7, 0.7]) ant.set_qvel([0] * 8) f = [0.1] * 8 acc = ant.compute_forward_dynamics([0.1] * 8) print(acc)
def success_test(self): self.scene.remove_articulation(self.robot) self.boxes[0].set_pose(Pose([0.6, 0, 0.03])) self.boxes[1].set_pose(Pose([0.61, 0.01, 0.15])) self.boxes[2].set_pose(Pose([0.59, -0.01, 0.3]))
def act(self, env: FinalEnv, current_timestep: int): r1, r2, c1, c2, c3, c4 = env.get_agents() pf_left = f = r1.get_compute_functions()['passive_force'](True, True, False) pf_right = f = r2.get_compute_functions()['passive_force'](True, True, False) if self.phase == 0: t1 = [2, 1, 0, -1.5, -1, 1, -2] t2 = [-2, 1, 0, -1.5, 1, 1, -2] r1.set_action(t1, [0] * 7, pf_left) r2.set_action(t2, [0] * 7, pf_right) if np.allclose(r1.get_observation()[0], t1, 0.05, 0.05) and np.allclose( r2.get_observation()[0], t2, 0.05, 0.05): self.phase = 1 self.counter = 0 self.selected_x = None if self.phase == 1: self.counter += 1 if (self.counter == 1): selected = self.pick_box(c4) self.selected_x = selected[0] if self.selected_x is None: return False target_pose_left = Pose([self.selected_x, 0.5, 0.67], euler2quat(np.pi, -np.pi / 3, -np.pi / 2)) self.diff_drive(r1, 9, target_pose_left) target_pose_right = Pose([self.selected_x, -0.5, 0.6], euler2quat(np.pi, -np.pi / 3, np.pi / 2)) self.diff_drive(r2, 9, target_pose_right) if self.counter == 2000 / 5: self.phase = 2 self.counter = 0 pose = r1.get_observation()[2][9] p, q = pose.p, pose.q p[1] = 0.07 self.pose_left = Pose(p, q) pose = r2.get_observation()[2][9] p, q = pose.p, pose.q p[1] = -0.07 self.pose_right = Pose(p, q) if self.phase == 2: self.counter += 1 self.diff_drive(r1, 9, self.pose_left) self.diff_drive(r2, 9, self.pose_right) if self.counter == 2000 / 5: self.phase = 3 pose = r2.get_observation()[2][9] p, q = pose.p, pose.q p[2] += 0.2 self.pose_right = Pose(p, q) pose = r1.get_observation()[2][9] p, q = pose.p, pose.q p[1] = 0.5 q = euler2quat(np.pi, -np.pi / 2, -np.pi / 2) self.pose_left = Pose(p, q) self.counter = 0 if self.phase == 3: self.counter += 1 self.diff_drive(r1, 9, self.pose_left) self.diff_drive(r2, 9, self.pose_right) if self.counter == 200 / 5: self.phase = 4 self.counter = 0 if self.phase == 4: self.counter += 1 if (self.counter < 3000 / 5): pose = r2.get_observation()[2][9] p, q = pose.p, pose.q q = euler2quat(np.pi, -np.pi / 1.5, quat2euler(q)[2]) self.diff_drive2(r2, 9, Pose(p, q), [4, 5, 6], [0, 0, 0, -1, 0], [0, 1, 2, 3, 4]) elif (self.counter < 6000 / 5): pose = r2.get_observation()[2][9] p, q = pose.p, pose.q q = euler2quat(np.pi, -np.pi / 1.5, quat2euler(q)[2]) self.diff_drive2(r2, 9, Pose(p, q), [4, 5, 6], [0, 0, 1, -1, 0], [0, 1, 2, 3, 4]) elif (self.counter < 9000 / 5): p = [-1, 0, 1.5] q = euler2quat(0, -np.pi / 1.5, 0) self.diff_drive(r2, 9, Pose(p, q)) else: self.phase = 0
def create_ant_builder(scene): from transforms3d.quaternions import axangle2quat as aa builder = scene.create_articulation_builder() body = builder.create_link_builder() body.add_sphere_shape(Pose(), 0.25) body.add_sphere_visual(Pose(), 0.25) body.add_capsule_shape(Pose([0.141, 0, 0]), 0.08, 0.141) body.add_capsule_visual(Pose([0.141, 0, 0]), 0.08, 0.141) body.add_capsule_shape(Pose([-0.141, 0, 0]), 0.08, 0.141) body.add_capsule_visual(Pose([-0.141, 0, 0]), 0.08, 0.141) body.add_capsule_shape(Pose([0, 0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141) body.add_capsule_visual(Pose([0, 0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141) body.add_capsule_shape(Pose([0, -0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141) body.add_capsule_visual(Pose([0, -0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141) body.set_name("body") l1 = builder.create_link_builder(body) l1.set_name("l1") l1.set_joint_name("hip_1") l1.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]], Pose([0.282, 0, 0], [0.7071068, 0, 0.7071068, 0]), Pose([0.141, 0, 0], [-0.7071068, 0, 0.7071068, 0]), 0.1) l1.add_capsule_shape(Pose(), 0.08, 0.141) l1.add_capsule_visual(Pose(), 0.08, 0.141) l2 = builder.create_link_builder(body) l2.set_name("l2") l2.set_joint_name("hip_2") l2.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]], Pose([-0.282, 0, 0], [0, -0.7071068, 0, 0.7071068]), Pose([0.141, 0, 0], [-0.7071068, 0, 0.7071068, 0]), 0.1) l2.add_capsule_shape(Pose(), 0.08, 0.141) l2.add_capsule_visual(Pose(), 0.08, 0.141) l3 = builder.create_link_builder(body) l3.set_name("l3") l3.set_joint_name("hip_3") l3.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]], Pose([0, 0.282, 0], [0.5, -0.5, 0.5, 0.5]), Pose([0.141, 0, 0], [0.7071068, 0, -0.7071068, 0]), 0.1) l3.add_capsule_shape(Pose(), 0.08, 0.141) l3.add_capsule_visual(Pose(), 0.08, 0.141) l4 = builder.create_link_builder(body) l4.set_name("l4") l4.set_joint_name("hip_4") l4.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]], Pose([0, -0.282, 0], [0.5, 0.5, 0.5, -0.5]), Pose([0.141, 0, 0], [0.7071068, 0, -0.7071068, 0]), 0.1) l4.add_capsule_shape(Pose(), 0.08, 0.141) l4.add_capsule_visual(Pose(), 0.08, 0.141) f1 = builder.create_link_builder(l1) f1.set_name("f1") f1.set_joint_name("ankle_1") f1.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]], Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]), Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1) f1.add_capsule_shape(Pose(), 0.08, 0.282) f1.add_capsule_visual(Pose(), 0.08, 0.282) f2 = builder.create_link_builder(l2) f2.set_name("f2") f2.set_joint_name("ankle_2") f2.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]], Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]), Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1) f2.add_capsule_shape(Pose(), 0.08, 0.282) f2.add_capsule_visual(Pose(), 0.08, 0.282) f3 = builder.create_link_builder(l3) f3.set_name("f3") f3.set_joint_name("ankle_3") f3.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]], Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]), Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1) f3.add_capsule_shape(Pose(), 0.08, 0.282) f3.add_capsule_visual(Pose(), 0.08, 0.282) f4 = builder.create_link_builder(l4) f4.set_name("f4") f4.set_joint_name("ankle_4") f4.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]], Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]), Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1) f4.add_capsule_shape(Pose(), 0.08, 0.282) f4.add_capsule_visual(Pose(), 0.08, 0.282) return builder
def build_camreas(self): b = self.scene.create_actor_builder() b.add_visual_from_file('./assets/camera.dae') cam_front = b.build(True, name="camera") pitch = np.deg2rad(-15) cam_front.set_pose( Pose([1, 0, 1], qmult(axangle2quat([0, 0, 1], np.pi), axangle2quat([0, -1, 0], pitch)))) c = self.scene.add_mounted_camera('front_camera', cam_front, Pose(), 640, 480, 0, np.deg2rad(45), 0.1, 10) self.front_camera = Camera(c) b = self.scene.create_actor_builder() b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1])) b.build(True, name="tripod").set_pose(Pose([1, 0, 1])) b = self.scene.create_actor_builder() b.add_visual_from_file('./assets/camera.dae') cam_left = b.build(True, name="camera") pitch = np.deg2rad(-15) cam_left.set_pose( Pose([0, 1.5, 1], qmult(axangle2quat([0, 0, 1], -np.pi / 2), axangle2quat([0, -1, 0], pitch)))) c = self.scene.add_mounted_camera('left_camera', cam_left, Pose(), 640, 480, 0, np.deg2rad(45), 0.1, 10) self.left_camera = Camera(c) b = self.scene.create_actor_builder() b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1])) b.build(True, name="tripod").set_pose(Pose([0, 1.5, 1])) b = self.scene.create_actor_builder() b.add_visual_from_file('./assets/camera.dae') cam_right = b.build(True, name="camera") pitch = np.deg2rad(-15) cam_right.set_pose( Pose([0, -1.5, 1], qmult(axangle2quat([0, 0, 1], np.pi / 2), axangle2quat([0, -1, 0], pitch)))) c = self.scene.add_mounted_camera('right_camera', cam_right, Pose(), 640, 480, 0, np.deg2rad(45), 0.1, 10) self.right_camera = Camera(c) b = self.scene.create_actor_builder() b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1])) b.build(True, name="tripod").set_pose(Pose([0, -1.5, 1])) b = self.scene.create_actor_builder() b.add_visual_from_file('./assets/camera.dae') cam_top = b.build(True, name="camera") pitch = np.deg2rad(-90) cam_top.set_pose(Pose([-0.5, 0, 3], axangle2quat([0, -1, 0], pitch))) c = self.scene.add_mounted_camera('top_camera', cam_top, Pose(), 640, 480, 0, np.deg2rad(45), 0.1, 10) self.top_camera = Camera(c)
def before_step(self, env, task: DexPushTask): robot: PandaArm = env.agents[ARM_NAME] box: Box = env.agents[BOX_NAME] if self.phase is Phase.rotate_toward_box: if self.init_control is True: box2base = robot.observation['poses'][0].inv() * box.observation['pose'] p = box2base.p theta = np.arctan(p[1] / p[0]) if p[0] < 0 and p[1] < 0: theta -= np.pi elif p[0] < 0 and p[1] > 0: theta += np.pi self.init_control = False self.tg_pose = Pose(robot.observation['poses'][0].p, euler.euler2quat(0, 0, theta)) self.drive_to_pose(robot, self.tg_pose, joint_index=1, theta_thresh=1e-4, theta_abs_thresh=1e-5) if self.drive[robot] is False: self.phase = Phase.move2above self.prep_drive(robot) elif self.phase is Phase.move2above: if self.init_control is True: box2wd = box.observation['pose'] goal2box = box2wd.inv() * Pose(self.goal2wd) robot2box = box2wd.inv() * robot.observation['poses'][0] direct = np.array([0, 0, 0]) if np.min(np.abs(goal2box.p)) > 5e-3: indx = np.argmin(np.abs(robot2box.p[:2])) if np.abs(goal2box.p[indx]) < 8e-4: indx = 1 - indx else: indx = np.argmax(np.abs(goal2box.p[:2])) direct[indx] = 1 dist = goal2box.p @ direct dist = np.min((abs(dist), 0.2)) * np.sign(dist) quat = Pose([0]*3, self.up_right_q) if direct[1] == 1: quat = quat * Pose([0]*3, euler.euler2quat(0, 0, np.pi/2)) ee2pt = Pose([0,0,0.1], quat.q) gripper_width = 0.013 self.plan2wd = box2wd * Pose(-direct * np.sign(dist) * (box.box_size + gripper_width) + dist * direct) * ee2pt self.push2wd = box2wd * Pose(-direct * np.sign(dist) * (box.box_size + gripper_width)) * ee2pt self.tg_pose = Pose([0,0,0.1]) * box2wd * Pose(-direct * np.sign(dist) * (box.box_size + gripper_width + 0.02)) * ee2pt self.init_control = False self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0], [0, 0]), dist_abs_thresh=1e-6, dist_thresh=1e-6, theta_abs_thresh=1e-3, theta_thresh=1e-3) if self.drive[robot] is False: self.phase = Phase.move2box self.prep_drive(robot) elif self.phase is Phase.move2box: if self.init_control is True: self.tg_pose = self.push2wd self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0], [0, 0]), dist_abs_thresh=1e-5, dist_thresh=1e-5, theta_abs_thresh=1e-3, theta_thresh=1e-3) if self.drive[robot] is False: self.prep_drive(robot) self.phase = Phase.push2goal elif self.phase is Phase.push2goal: if self.init_control is True: self.tg_pose = self.plan2wd self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0], [0, 0]), dist_abs_thresh=1e-5, dist_thresh=1e-5, theta_abs_thresh=1e-3, theta_thresh=1e-3) if self.drive[robot] is False or task.get_trajectory_status()['succeeded'] is True: self.prep_drive(robot) self.phase = Phase.moveUp elif self.phase is Phase.moveUp: if self.init_control is True: self.tg_pose = Pose([0,0,0.1]) * robot.observation['poses'][-3] self.init_control = False self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0], [0, 0]), dist_abs_thresh=1e-4) if self.drive[robot] is False: self.prep_drive(robot) status = task.get_trajectory_status() if status['succeeded'] is False and status['valid_push'] is True: self.phase = Phase.move2above else: self.phase = Phase.evaluate elif self.phase is Phase.evaluate: print(task.get_trajectory_status()) self.phase = Phase.done
sim.set_renderer(renderer) scene = sim.create_scene() scene.set_timestep(1 / 60) scene.set_ambient_light([0.5, 0.5, 0.5]) scene.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5]) scene.add_point_light([1, 2, 2], [1, 1, 1]) scene.add_point_light([1, -2, 2], [1, 1, 1]) scene.add_point_light([-1, 0, 1], [1, 1, 1]) near, far = 0.1, 100 camera_mount_actor = scene.create_actor_builder().build(is_kinematic=True) camera = scene.add_mounted_camera( "first_camera", camera_mount_actor, Pose(), 640, 480, np.deg2rad(35), np.deg2rad(35), near, far, ) pos = np.array([-2, -2, 3]) forward = -pos / np.linalg.norm(pos) left = np.cross([0, 0, 1], forward) left = left / np.linalg.norm(left) up = np.cross(forward, left) mat44 = np.eye(4) mat44[:3, :3] = np.array([forward, left, up]).T
"panda_leftfinger": { "patch_radius": 0.5 }, "panda_rightfinger": { "shape": { 0: { "patch_radius": 0.4 } } }, } } robot = loader.load("../assets_local/robot/panda.urdf", config) model = robot.create_pinocchio_model() targetPose = Pose([-0.01, -0.39, 0.96], [0.21, 0.73, 0.41, 0.5]) result, success, error = model.compute_inverse_kinematics(9, targetPose) print('success: ', success) print('error: ', error) # robot.set_qpos(result) loader.load("../assets/robot/sapien_gripper.urdf") render_controller.show_window() render_controller.set_camera_position(-5, 0, 0) render_controller.set_current_scene(scene) steps = 0 while not render_controller.should_quit: scene.update_render() for i in range(4):
renderer = sapien.OptifuserRenderer() sim.set_renderer(renderer) controller = sapien.OptifuserController(renderer) controller.show_window() s0 = sim.create_scene() s0.add_ground(-1) s0.set_timestep(1 / 60) s0.set_ambient_light([0.5, 0.5, 0.5]) s0.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5]) builder = s0.create_actor_builder() # builder.add_box_shape() # builder.add_box_visual() builder.add_multiple_convex_shapes_from_file( '/home/fx/source/py-vhacd/example/decomposed.obj') builder.add_visual_from_file('/home/fx/source/py-vhacd/example/decomposed.obj') actor = builder.build() actor.set_pose(Pose([0, 0, 2])) controller.set_current_scene(s0) while not controller.should_quit: s0.update_render() s0.step() controller.render()
def create_bin(self, p, r, size, thickness): if self.bin is not None: self.scene.remove_actor(self.bin) self.bin_size = size b = self.scene.create_actor_builder() b.add_box_shape(Pose([0, 0, thickness / 2]), [size[0] / 2, size[1] / 2, thickness / 2]) b.add_box_visual(Pose([0, 0, thickness / 2]), [size[0] / 2, size[1] / 2, thickness / 2], [0.1, 0.8, 1]) b.add_box_shape(Pose([0, size[1] / 2, size[2] / 2]), [size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2]) b.add_box_visual(Pose([0, size[1] / 2, size[2] / 2]), [size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2], [0.1, 0.8, 1]) b.add_box_shape(Pose([0, -size[1] / 2, size[2] / 2]), [size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2]) b.add_box_visual(Pose([0, -size[1] / 2, size[2] / 2]), [size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2], [0.1, 0.8, 1]) b.add_box_shape(Pose([size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2]) b.add_box_visual(Pose([size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2], [0.1, 0.8, 1]) b.add_box_shape(Pose([-size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2]) b.add_box_visual(Pose([-size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2], [0.1, 0.8, 1]) self.bin = b.build(True, name="bin") self.bin.set_pose(Pose(p, axangle2quat([0, 0, 1], r)))
forward /= np.linalg.norm(forward) out_info['gripper_forward_direction_world'] = forward.tolist() forward_cam = np.linalg.inv(cam.get_metadata()['mat44'][:3, :3]) @ forward out_info['gripper_forward_direction_camera'] = forward_cam.tolist() rotmat = np.eye(4).astype(np.float32) rotmat[:3, 0] = forward rotmat[:3, 1] = left rotmat[:3, 2] = up final_dist = 0.1 if primact_type == 'pushing-left' or primact_type == 'pushing-up': final_dist = 0.11 final_rotmat = np.array(rotmat, dtype=np.float32) final_rotmat[:3, 3] = position_world - action_direction_world * final_dist final_pose = Pose().from_transformation_matrix(final_rotmat) out_info['target_rotmat_world'] = final_rotmat.tolist() start_rotmat = np.array(rotmat, dtype=np.float32) start_rotmat[:3, 3] = position_world - action_direction_world * 0.15 start_pose = Pose().from_transformation_matrix(start_rotmat) out_info['start_rotmat_world'] = start_rotmat.tolist() action_direction = None if 'left' in primact_type: action_direction = forward elif 'up' in primact_type: action_direction = left if action_direction is not None: end_rotmat = np.array(rotmat, dtype=np.float32)
def check_inside_bin(self, point): size = self.bin_size [x, y, z] = self.bin.pose.inv().transform(Pose(point)).p return -size[0] / 2 < x < size[0] / 2 and \ -size[1] / 2 < y < size[1] / 2 and \ 0 < z < size[2]
def act(self, env: FinalEnv, current_timestep: int): """called at each (actionable) time step to set robot actions. return False to indicate that the agent decides to move on to the next environment. Returning False early could mean a lower success rate (correctly placed boxes / total boxes), but it can also save you some time, so given a fixed total time budget, you may be able to place more boxes. """ # robot_left, robot_right, camera_front, camera_left, camera_right, camera_top = env.get_agents() robot_left, robot_right, c1, c2, c3, c4 = env.get_agents() pf_left = f = robot_left.get_compute_functions()['passive_force'](True, True, False) pf_right = f = robot_right.get_compute_functions()['passive_force'](True, True, False) if self.phase == 0: self.counter += 1 t1 = [2, 1, 0, -1.5, -1, 1, -2.7] t2 = [-2, 1, 0, -1.5, 1, 1, -2] robot_left.set_action(t1, [0] * 7, pf_left) robot_right.set_action(t2, [0] * 7, pf_right) if np.allclose(robot_left.get_observation()[0], t1, 0.05, 0.05) and np.allclose( robot_right.get_observation()[0], t2, 0.05, 0.05): self.phase = 1 self.counter = 0 self.selected_x = None self.spade_width, self.spade_length = self.get_spade(c4) print(self.spade_width, self.spade_length) if (self.counter > 8000/5): self.counter = 0 return False if self.phase == 1: # print(11111) self.counter += 1 if (self.counter == 1): self.selected_x, self.selected_z = self.pick_box(c4) print(self.selected_x,self.selected_z) if self.selected_x is None: self.counter = 0 self.phase = 0 return False # change 0.67 to 0.69 target_pose_left = Pose([-0.3, 0.1, 0.55], euler2quat(np.pi, -np.pi / 3, -np.pi/2 )) self.diff_drive(robot_left, 9, target_pose_left) target_pose_right = Pose([-0.3, 0.1, 0.55], euler2quat(np.pi, -np.pi / 3, np.pi/2)) self.diff_drive(robot_right, 9, target_pose_right) if self.counter == 2000 / 5: self.phase = 2 self.counter = 0 pose = robot_left.get_observation()[2][9] p, q = pose.p, pose.q p[0] = 1 self.pose_left = Pose(p, q) pose = robot_right.get_observation()[2][9] p, q = pose.p, pose.q p[0] = 1 self.pose_right = Pose(p, q) if self.phase == 2: # print(22222) self.counter += 1 self.diff_drive(robot_left, 9, self.pose_left) self.diff_drive(robot_right, 9, self.pose_right) if self.counter == 600 / 5: self.phase = 3 self.counter = 0 if self.phase == 3: self.counter += 1 if (self.counter < 400/5): pose = robot_left.get_observation()[2][9] p, q = pose.p, pose.q p[0] = -3 self.diff_drive(robot_left, 9, Pose(p, q)) pose = robot_right.get_observation()[2][9] p, q = pose.p, pose.q p[0] = -3 self.diff_drive(robot_right, 9, Pose(p, q)) else: t1 = [2, 1, 0, -1.5, -1, 1, -2] t2 = [-2, 1, 0, -1.5, 1, 1, -2] robot_left.set_action(t1, [0] * 7, pf_left) robot_right.set_action(t2, [0] * 7, pf_right) if np.allclose(robot_left.get_observation()[0], t1, 0.05, 0.05) and np.allclose( robot_right.get_observation()[0], t2, 0.05, 0.05): self.phase = 4 self.counter = 0 self.selected_x = None self.spade_width, self.spade_length = self.get_spade(c4) print(self.spade_width, self.spade_length) if (self.counter > 8000/5): self.counter = 0 return False if self.phase == 4: # print(11111) self.counter += 1 if (self.counter == 1): self.selected_x, self.selected_z = self.pick_box(c4) print(self.selected_x,self.selected_z) if self.selected_x is None: self.counter = 0 self.phase = 0 return False # change 0.67 to 0.69 target_pose_left = Pose([self.selected_x, 0.6, self.selected_z + 0.65], euler2quat(np.pi, -np.pi / 3, -np.pi / 2)) self.diff_drive(robot_left, 9, target_pose_left) target_pose_right = Pose([self.selected_x, -0.6, self.selected_z + 0.55], euler2quat(np.pi, -np.pi / 3, np.pi / 2)) self.diff_drive(robot_right, 9, target_pose_right) if self.counter == 1500 / 5: self.phase = 5 self.counter = 0 pose = robot_left.get_observation()[2][9] p, q = pose.p, pose.q p[1] = self.spade_length/2 self.pose_left = Pose(p, q) pose = robot_right.get_observation()[2][9] p, q = pose.p, pose.q p[1] = - self.spade_length / 2 self.pose_right = Pose(p, q) if self.phase == 5: # print(22222) self.counter += 1 self.diff_drive(robot_left, 9, self.pose_left) self.diff_drive(robot_right, 9, self.pose_right) if self.counter == 2500 / 5: self.phase = 6 pose = robot_right.get_observation()[2][9] p, q = pose.p, pose.q # p[2] += 0.1 self.pose_right = Pose(p, q) # pose = robot_left.get_observation()[2][9] # p, q = pose.p, pose.q # p[1] = -0.3 # p[2] += 0.3 # q = euler2quat(np.pi, -np.pi / 8, -np.pi / 2) # self.pose_left = Pose(p, q) self.counter = 0 if self.phase == 6: # print(33333) pose = robot_left.get_observation()[2][9] p, q = pose.p, pose.q # p[1] -= 0.1 p[2] += 0.1 q = euler2quat(np.pi, -np.pi / 4, -np.pi /2) self.pose_left = Pose(p, q) self.counter += 1 self.diff_drive(robot_left, 9, self.pose_left) if self.counter == 200 / 5: self.phase = 7 self.counter = 0 if self.phase == 7: # print(self.counter) # self.diff_drive(robot_right, 9, self.pose_right) self.counter += 1 if (self.counter < 200 / 5): pose = robot_left.get_observation()[2][9] p, q = pose.p, pose.q p[1] += 5 p[2] += 5 # q = euler2quat(np.pi, 0, -np.pi / 4) # q = euler2quat(np.pi, -np.pi / 4, -np.pi /2) self.diff_drive(robot_left, 9, Pose(p, q)) elif (self.counter < 300 / 5): pose = robot_right.get_observation()[2][9] p, q = pose.p, pose.q q = euler2quat(0,0 ,np.pi/4) self.diff_drive(robot_right, 9, Pose(p, q)) elif (self.counter < 6500 / 5): p = [self.top_view[0], self.top_view[1] + self.spade_length /3, 2.0*self.front_view[2]] q = euler2quat(0, -np.pi / 1.5, 0) self.diff_drive(robot_right, 9, Pose(p, q)) # if self.counter == 500/5: # cur_box = self.check_box(c2) # print(self.box_num, cur_box) # if (cur_box >= self.box_num): # self.phase = 0 # return False elif (self.counter < 9000 / 5): pose = robot_right.get_observation()[2][9] p = pose.p q = euler2quat(0, -np.pi / 1.5, 0) self.diff_drive(robot_right, 9, Pose(p, q)) else: self.phase = 0 self.counter = 0 return False
def after_substep(self, env) -> None: assert self._initialized, "task not initialized" if self._box_valid_tumble is False or self._suceeded or not self._tracking: self._tracking = False return box_pose = self._box.observation['pose'] # calculate how much "flipped" rot_mat = quat2mat(box_pose.q) box_up = rot_mat[:3, 2] box_up /= np.linalg.norm(box_up) # check height if box_pose.p[2] > self._box_height_thresh: self._suceeded = False self._box_valid_tumble = False self._failed_reason = FAIL_REASON_TOO_HIGH return tiled = np.arccos(box_up[2]) > TILT_THRESHOLD theta_x, theta_y = np.pi / 2, np.pi / 2 if tiled: theta_x, theta_y = np.arccos(self._last_basis @ box_up) else: self._last_basis = rot_mat[:3, :2] self._last_basis[2, :] = 0 self._last_basis /= np.linalg.norm(self._last_basis, axis=0) self._last_basis = self._last_basis.T tumble_x = theta_x < np.pi / 2 - TILT_THRESHOLD tumble_y = theta_y < np.pi / 2 - TILT_THRESHOLD # check tumble self._box_valid_tumble = not (tumble_x and tumble_y) if self._box_valid_tumble is False: self._suceeded = False self._failed_reason = FAIL_REASON_DIAG_FLIP return # check edge center if tumble_x or tumble_y: center2pt1 = np.array([0, 0, -self._box.box_size]) center2pt2 = center2pt1.copy() if tumble_x: center2pt1[0] = np.sign(theta_x) * self._box.box_size center2pt1[1] = self._box.box_size/2 center2pt2[0] = np.sign(theta_x) * self._box.box_size center2pt2[1] = -self._box.box_size/2 else: center2pt1[1] = np.sign(theta_y) * self._box.box_size center2pt1[0] = self._box.box_size/2 center2pt2[1] = np.sign(theta_y) * self._box.box_size center2pt2[0] = -self._box.box_size/2 center2pt1_pose = Pose(center2pt1) edge_pt1 = (box_pose * center2pt1_pose).p center2pt2_pose = Pose(center2pt2) edge_pt2 = (box_pose * center2pt2_pose).p if self._last_edge_pt1 is not None and self._last_edge_pt2 is not None: dist1 = np.linalg.norm(self._last_edge_pt1 - edge_pt1) dist2 = np.linalg.norm(self._last_edge_pt2 - edge_pt2) if dist1 > BOX_EDGE_DISP_THRESHOLD or dist2 > BOX_EDGE_DISP_THRESHOLD: self._box_valid_tumble = False self._failed_reason = FAIL_REASON_EDGE_MOVED return else: self._last_edge_pt1 = edge_pt1 self._last_edge_pt2 = edge_pt2 else: self._last_edge_pt1 = None self._last_edge_pt2 = None # check succeeded if tumble_x ^ tumble_y and \ (theta_x < np.pi/2 - FULL_TUMBLE_THRESHOLD or theta_y < np.pi/2 - FULL_TUMBLE_THRESHOLD): self._suceeded = True
def build_table(self): b = self.scene.create_actor_builder() b.add_multiple_convex_shapes_from_file('./assets/bigboard.obj', Pose([-0.3, 0, 0])) b.add_visual_from_file('./assets/bigboard.obj', Pose([-0.3, 0, 0])) self.table = b.build_static(name="table")
def setup_target(self, position, size): self.target = (position, size) builder = self.scene.create_actor_builder() builder.add_box_visual(size=[size] * 3, color=[0, 1, 1]) indicator = builder.build_static('goal') indicator.set_pose(Pose([position[0], position[1], -size + 0.001]))