def go_to_push_start(self, vec_env, z_offset=0.09): for env_index, env_ptr in enumerate(vec_env._scene.env_ptrs): block_ah = vec_env._scene.ah_map[env_index][vec_env._block_name] block_transform = vec_env._block.get_rb_transforms( env_ptr, block_ah)[0] ee_transform = vec_env._frankas[env_index].get_ee_transform( env_ptr, vec_env._franka_name) grasp_transform = gymapi.Transform(p=block_transform.p, r=ee_transform.r) pre_grasp_transfrom = gymapi.Transform(p=grasp_transform.p, r=grasp_transform.r) pre_grasp_transfrom.p.z += z_offset pre_grasp_transfrom.p.y -= 0.01 rt = transform_to_RigidTransform(pre_grasp_transfrom) rot_matrix = RigidTransform.y_axis_rotation(np.pi / 2) rt.rotation = np.dot(rot_matrix, rt.rotation) pre_grasp_transfrom = RigidTransform_to_transform(rt) stiffness = 1e3 vec_env._frankas[env_index].set_attractor_props( env_index, env_ptr, vec_env._franka_name, { 'stiffness': stiffness, 'damping': 6 * np.sqrt(stiffness) }) vec_env._frankas[env_index].set_ee_transform( env_ptr, env_index, vec_env._franka_name, pre_grasp_transfrom)
def straighten_transform(rt): angles = rt.euler_angles roll_off = angles[0] pitch_off = angles[1] roll_fix = RigidTransform( rotation=RigidTransform.x_axis_rotation(np.pi - roll_off), from_frame=rt.from_frame, to_frame=rt.from_frame) pitch_fix = RigidTransform( rotation=RigidTransform.y_axis_rotation(pitch_off), from_frame=rt.from_frame, to_frame=rt.from_frame) new_rt = rt * roll_fix * pitch_fix return new_rt
arm = self.left_arm else: arm = self.right_arm arm.set_start_state_to_current_state() arm.set_pose_target(pose) arm.plan() arm.go() if __name__ == '__main__': # initialize the ROS node rospy.init_node('Grasp_Execution_Server') # Tf gripper frame to grasp cannonical frame (y-axis = grasp axis, x-axis = palm axis) rospy.loginfo('Loading T_gripper_grasp') rotation = RigidTransform.y_axis_rotation(float(np.pi / 2)) T_gripper_grasp = RigidTransform(rotation, from_frame='gripper', to_frame='grasp') # load camera tf rospy.loginfo('Loading T_camera_world') T_camera_world = RigidTransform.load(CFG_PATH + 'kinect_to_world.tf') # Initialize Baxter rospy.loginfo('Initializing Baxter') grasp_executer = GraspExecuter(MAX_VELOCITY) # Add collision meshes for moveit planner rospy.loginfo('Constructing planning scene') camera_pose = PoseStamped()