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()
