def setup_arm_mp(self): """ Set up arm motion planner """ if self.robot_type == 'Fetch': self.arm_default_joint_positions = (0.10322468280792236, -1.414019864768982, 1.5178184935241699, 0.8189625336474915, 2.200358942909668, 2.9631312579803466, -1.2862852996643066, 0.0008453550418615341) self.arm_joint_ids = joints_from_names(self.robot_id, [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ]) elif self.robot_type == 'Movo': self.arm_default_joint_positions = (0.205, -1.50058731470836, -1.3002625076695704, 0.5204845864369407, -2.6923805472917626, -0.02678584326934146, 0.5065742552588746, -1.562883631882778) self.arm_joint_ids = joints_from_names(self.robot_id, [ "linear_joint", "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", ]) self.arm_joint_ids_all = get_moving_links(self.robot_id, self.arm_joint_ids) self.arm_joint_ids_all = [ item for item in self.arm_joint_ids_all if item != self.robot.end_effector_part_index() ] self.arm_ik_threshold = 0.05 self.mp_obstacles = [] if type(self.env.scene) == StaticIndoorScene: if self.env.scene.mesh_body_id is not None: self.mp_obstacles.append(self.env.scene.mesh_body_id) elif type(self.env.scene) == InteractiveIndoorScene: self.mp_obstacles.extend(self.env.scene.get_body_ids())
def robot_specific_reset(self): super(Fetch, self).robot_specific_reset() # roll the arm to its body robot_id = self.robot_ids[0] arm_joints = joints_from_names(robot_id, [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ]) rest_position = (0.02, np.pi / 2.0 - 0.4, np.pi / 2.0 - 0.1, -0.4, np.pi / 2.0 + 0.1, 0.0, np.pi / 2.0, 0.0) # might be a better pose to initiate manipulation # rest_position = (0.30322468280792236, -1.414019864768982, # 1.5178184935241699, 0.8189625336474915, # 2.200358942909668, 2.9631312579803466, # -1.2862852996643066, 0.0008453550418615341) set_joint_positions(robot_id, arm_joints, rest_position)
def load(self): """ Load the robot into pybullet. Filter out unnecessary self collision due to modeling imperfection in the URDF """ ids = super(JR2_Kinova, self).load() robot_id = self.robot_ids[0] disable_collision_names = [ ['base_chassis_joint', 'pan_joint'], ['base_chassis_joint', 'tilt_joint'], ['base_chassis_joint', 'camera_joint'], ['jr2_fixed_body_joint', 'pan_joint'], ['jr2_fixed_body_joint', 'tilt_joint'], ['jr2_fixed_body_joint', 'camera_joint'], ] for names in disable_collision_names: link_a, link_b = joints_from_names(robot_id, names) p.setCollisionFilterPair(robot_id, robot_id, link_a, link_b, 0) return ids
def load(self): """ Load the robot into pybullet. Filter out unnecessary self collision due to modeling imperfection in the URDF """ ids = super(Fetch, self).load() robot_id = self.robot_ids[0] disable_collision_names = [ ['torso_lift_joint', 'shoulder_lift_joint'], ['torso_lift_joint', 'torso_fixed_joint'], ['caster_wheel_joint', 'estop_joint'], ['caster_wheel_joint', 'laser_joint'], ['caster_wheel_joint', 'torso_fixed_joint'], ['caster_wheel_joint', 'l_wheel_joint'], ['caster_wheel_joint', 'r_wheel_joint'], ] for names in disable_collision_names: link_a, link_b = joints_from_names(robot_id, names) p.setCollisionFilterPair(robot_id, robot_id, link_a, link_b, 0) return ids
def main(): config = parse_config('../configs/fetch_p2p_nav.yaml') s = Simulator(mode='gui', physics_timestep=1 / 240.0) scene = EmptyScene() s.import_scene(scene) fetch = Fetch(config) s.import_robot(fetch) robot_id = fetch.robot_ids[0] arm_joints = joints_from_names(robot_id, [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ]) #finger_joints = joints_from_names(robot_id, ['l_gripper_finger_joint', 'r_gripper_finger_joint']) fetch.robot_body.reset_position([0, 0, 0]) fetch.robot_body.reset_orientation([0, 0, 1, 0]) x, y, z = fetch.get_end_effector_position() #set_joint_positions(robot_id, finger_joints, [0.04,0.04]) print(x, y, z) visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius=0.02) marker = p.createMultiBody(baseVisualShapeIndex=visual_marker) #max_limits = [0,0] + get_max_limits(robot_id, arm_joints) + [0.05,0.05] #min_limits = [0,0] + get_min_limits(robot_id, arm_joints) + [0,0] #rest_position = [0,0] + list(get_joint_positions(robot_id, arm_joints)) + [0.04,0.04] max_limits = [0, 0] + get_max_limits(robot_id, arm_joints) min_limits = [0, 0] + get_min_limits(robot_id, arm_joints) rest_position = [0, 0] + list(get_joint_positions(robot_id, arm_joints)) joint_range = list(np.array(max_limits) - np.array(min_limits)) joint_range = [item + 1 for item in joint_range] jd = [0.1 for item in joint_range] print(max_limits) print(min_limits) def accurateCalculateInverseKinematics(robotid, endEffectorId, targetPos, threshold, maxIter): sample_fn = get_sample_fn(robotid, arm_joints) set_joint_positions(robotid, arm_joints, sample_fn()) it = 0 while it < maxIter: jointPoses = p.calculateInverseKinematics(robotid, endEffectorId, targetPos, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=joint_range, restPoses=rest_position, jointDamping=jd) set_joint_positions(robotid, arm_joints, jointPoses[2:10]) ls = p.getLinkState(robotid, endEffectorId) newPos = ls[4] dist = np.linalg.norm(np.array(targetPos) - np.array(newPos)) if dist < threshold: break it += 1 print("Num iter: " + str(it) + ", threshold: " + str(dist)) return jointPoses while True: with Profiler("Simulation step"): fetch.robot_body.reset_position([0, 0, 0]) fetch.robot_body.reset_orientation([0, 0, 1, 0]) threshold = 0.01 maxIter = 100 joint_pos = accurateCalculateInverseKinematics( robot_id, fetch.parts['gripper_link'].body_part_index, [x, y, z], threshold, maxIter)[2:10] #set_joint_positions(robot_id, finger_joints, [0.04, 0.04]) s.step() keys = p.getKeyboardEvents() for k, v in keys.items(): if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_IS_DOWN)): x += 0.01 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_IS_DOWN)): x -= 0.01 if (k == p.B3G_UP_ARROW and (v & p.KEY_IS_DOWN)): y += 0.01 if (k == p.B3G_DOWN_ARROW and (v & p.KEY_IS_DOWN)): y -= 0.01 if (k == ord('z') and (v & p.KEY_IS_DOWN)): z += 0.01 if (k == ord('x') and (v & p.KEY_IS_DOWN)): z -= 0.01 p.resetBasePositionAndOrientation(marker, [x, y, z], [0, 0, 0, 1]) s.disconnect()