00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 import rospy
00022 from moveit_msgs.msg import Grasp
00023 from trajectory_msgs.msg import JointTrajectoryPoint
00024
00025 _sr_joint_names = [
00026 'FFJ1', 'FFJ2', 'FFJ3', 'FFJ4',
00027 'LFJ1', 'LFJ2', 'LFJ3', 'LFJ4', 'LFJ5',
00028 'MFJ1', 'MFJ2', 'MFJ3', 'MFJ4',
00029 'RFJ1', 'RFJ2', 'RFJ3', 'RFJ4',
00030 'THJ1', 'THJ2', 'THJ3', 'THJ4', 'THJ5',
00031 'WRJ1', 'WRJ2']
00032
00033 _sr_joint_names_j0 = [
00034 'LFJ0', 'LFJ3', 'LFJ4', 'LFJ5',
00035 'RFJ0', 'RFJ3', 'RFJ4',
00036 'MFJ0', 'MFJ3', 'MFJ4',
00037 'FFJ0', 'FFJ3', 'FFJ4',
00038 'THJ1', 'THJ2', 'THJ3', 'THJ4', 'THJ5',
00039 'WRJ1', 'WRJ2']
00040
00041 def _fix_j0(joints):
00042 """
00043 Convert joints targets using J1 and J2 to J0, which the controllers use.
00044 Useful if using joint_states to get grasp joint positions.
00045 """
00046 for finger in ['FFJ', 'LFJ', 'MFJ', 'RFJ']:
00047 if finger+'1' in joints and finger+'2' in joints:
00048 joints[finger+'0'] = joints[finger+'1'] + joints[finger+'2']
00049 del joints[finger+'1']
00050 del joints[finger+'2']
00051
00052 def mk_grasp(joints, pre_joints=None, fix_j0=False):
00053 """
00054 Generate a moveit_msgs/Grasp from a set of joint angles given as a dict
00055 of joint_name -> position.
00056 """
00057 if pre_joints is None:
00058 pre_joints = {}
00059
00060 sr_joint_names = _sr_joint_names
00061 if fix_j0:
00062 _fix_j0(joints)
00063 _fix_j0(pre_joints)
00064 sr_joint_names = _sr_joint_names_j0
00065 now = rospy.Time.now()
00066 grasp = Grasp()
00067 grasp.grasp_quality = 0.001
00068 grasp.grasp_pose.header.frame_id = "forearm"
00069 grasp.grasp_pose.header.stamp = now
00070 grasp.grasp_pose.pose.position.x = 0.01
00071 grasp.grasp_pose.pose.position.y = -0.045
00072 grasp.grasp_pose.pose.position.z = 0.321
00073 grasp.grasp_pose.pose.orientation.x = 0
00074 grasp.grasp_pose.pose.orientation.y = 0
00075 grasp.grasp_pose.pose.orientation.z = 0
00076 grasp.grasp_pose.pose.orientation.w = 0
00077
00078 grasp.pre_grasp_posture.header.stamp = now
00079 grasp.pre_grasp_posture.joint_names = sr_joint_names
00080 jtp = JointTrajectoryPoint()
00081 for jname in sr_joint_names:
00082 if jname in pre_joints:
00083 jtp.positions.append(pre_joints[jname])
00084 else:
00085 jtp.positions.append(0.0)
00086 grasp.pre_grasp_posture.points.append(jtp)
00087
00088 grasp.grasp_posture.header.stamp = now
00089 grasp.grasp_posture.joint_names = sr_joint_names
00090 jtp = JointTrajectoryPoint()
00091 for jname in sr_joint_names:
00092 if jname in joints:
00093 jtp.positions.append(joints[jname])
00094 else:
00095 jtp.positions.append(0.0)
00096 grasp.grasp_posture.points.append(jtp)
00097 return grasp