38 from nextage_ros_bridge
import nextage_client
40 import geometry_msgs.msg
43 from moveit_msgs.srv
import GetPositionIK
44 from moveit_msgs.msg
import PositionIKRequest
46 import nextage_rtm_playpattern
as nxtpp
51 Create a array of limb joint angles for the each waypoints 52 from data of the end effector postions and postures 53 using robot moving using RTM interface. 56 @param limb : limb to create a pattern, right arm 'rarm' or left arm 'larm' 57 @type pos_list : [[float,float,float],[float,float,float],...] 58 @param pos_list : Array of end effector positions (x,y,z) [m] 59 @type rpy_list : [[float,float,float],[float,float,float],...] 60 @param rpy_list : Array of end effector postures (r,p,y) [m] 61 @type tm_list : [float,float,...] 62 @param tm_list : Array of motion time lengths of the each pose [s] 64 @rtype : [[float,float,...],[float,float,...],...] 65 @return : Array of limb joint angles [rad] 68 for pos, rpy, tm
in zip(pos_list, rpy_list, tm_list):
69 robot.setTargetPose(limb, pos, rpy, tm)
70 robot.waitInterpolationOfGroup(limb)
72 joint_angles_deg = robot.getJointAngles()[3:9]
74 joint_angles_deg = robot.getJointAngles()[9:]
75 joint_angles_rad = [math.radians(angle_in_degree)
for angle_in_degree
in joint_angles_deg]
76 pattern_arm.append(joint_angles_rad)
82 Create a array of limb joint angles for the each waypoints 83 from data of the end effector postions and postures 84 using MoveIt! IK for computing joint angles. 87 @param limb : limb to create a pattern, right arm 'rarm' or left arm 'larm' 88 @type pos_list : [[float,float,float],[float,float,float],...] 89 @param pos_list : Array of end effector positions (x,y,z) [m] 90 @type rpy_list : [[float,float,float],[float,float,float],...] 91 @param rpy_list : Array of end effector postures (r,p,y) [m] 92 @type tm_list : [float,float,...] 93 @param tm_list : Array of motion time lengths of the each pose [s] 95 @rtype : [[float,float,...],[float,float,...],...] 96 @return : Array of limb joint angles [rad] 99 wpose = geometry_msgs.msg.Pose()
101 rospy.wait_for_service(
'compute_ik')
102 compute_ik = rospy.ServiceProxy(
'compute_ik', GetPositionIK)
105 limb_name =
"right_arm" 107 limb_name =
"left_arm" 109 joint_name = filter(
lambda n:n[0] == limb, robot.Groups)[0][1]
111 for pos, rpy, tm
in zip(pos_list, rpy_list, tm_list):
112 quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
113 wpose.position.x = pos[0]
114 wpose.position.y = pos[1]
115 wpose.position.z = pos[2]
116 wpose.orientation.x = quaternion[0]
117 wpose.orientation.y = quaternion[1]
118 wpose.orientation.z = quaternion[2]
119 wpose.orientation.w = quaternion[3]
120 req = PositionIKRequest()
121 req.group_name = limb_name
123 req.pose_stamped.pose = wpose
124 ret = compute_ik(req)
125 if ret.error_code.val != 1:
128 pattern_arm.append(map(
lambda n: n[1],filter(
lambda n:n[0]
in joint_name, zip(ret.solution.joint_state.name, ret.solution.joint_state.position))))
131 if __name__ ==
'__main__':
133 Main sequence for moving at waypoints with RTM interface 134 using MoveIt! IK for computing joint angles. 136 robot = nextage_client.NextageClient()
149 rect_xyzs = nxtpp.rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1])
150 rect_rpys = nxtpp.samePostureRPY([-3.073437, -1.569023, 3.073247], len(rect_xyzs))
151 rect_time = nxtpp.equalTimeList(10.0, len(rect_xyzs))
154 circ_xyzs = nxtpp.circularPositions(center=[0.35, 0.1, 0.1], radius=0.1 ,steps=12)
155 circ_rpys = nxtpp.samePostureRPY([-3.073437, -1.569023, 3.073247], len(circ_xyzs))
156 circ_time = nxtpp.equalTimeList(10.0, len(circ_xyzs))
163 positions_arm = rect_xyzs + circ_xyzs
164 rpys_arm = rect_rpys + circ_rpys
165 time_list = rect_time + circ_time
167 print(
'Limb: %s' % limb_name)
168 print(
'Positions (%d): %s' % (len(positions_arm), positions_arm))
169 print(
'RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))
170 print(
'Time List (%d): %s' % (len(time_list), time_list))
173 print(
'Generating Joint Angle Pattern')
175 play_pattern_time = time_list
178 print(
'Limb: %s' % limb_name)
179 print(
'Joint Angle Pattern [rad]: %s' % play_pattern_arm)
180 print(
'Motion Time Pattern [sec]: %s' % play_pattern_time)
185 print(
'Start: Play Pattern')
186 robot.playPatternOfGroup(limb_name, play_pattern_arm, play_pattern_time)
187 robot.waitInterpolationOfGroup(limb_name)
188 print(
'End: Play Pattern')
def setTargetPoseSequenceRTM(limb, pos_list, rpy_list, tm_list)
def setTargetPoseSequenceMoveIt(limb, pos_list, rpy_list, tm_list)