38 from nextage_ros_bridge
import nextage_client
44 Create a array of circular position coordinates 45 from a center position and a radius divided by steps. 47 @type center : [float, float, float] 48 @param center : Position (x,y,z) [m] coordinates of a circle center 50 @param radius : Radius length [m] 52 @param steps : Number of steps for dividing a circle 54 @rtype : [ [float, float, float], [float, float, float], ...] 55 @return : Array of circular position coordinates (x,y,z) [m] 56 Length of steps+1 (including an end position same as the start position) 59 step_rad = 2 * math.pi / steps
60 for i
in range(steps+1):
61 ang_rad = step_rad * i
62 px = center[0] - radius * math.cos(ang_rad)
63 py = center[1] + radius * math.sin(ang_rad)
65 positions_xyz.append([px,py,pz])
72 Create a array of rectangular position coordinates 73 from diagonal points of A and B. 75 @type dp_a : [float, float, float] 76 @param dp_a : Position (x,y,z) [m] coordinates of a diagonal point A 77 @type dp_a : [float, float, float] 78 @param dp_a : Position (x,y,z) [m] coordinates of a diagonal point B 80 @rtype : [ [float, float, float], [float, float, float], ...] 81 @return : Array of rectangular position coordinates (x,y,z) [m] 82 Length of 5 (including an end position same as the start position) 85 [ dp_a[0], dp_a[1], dp_a[2] ],
86 [ dp_a[0], dp_b[1], dp_a[2] ],
87 [ dp_b[0], dp_b[1], dp_b[2] ],
88 [ dp_b[0], dp_a[1], dp_b[2] ],
89 [ dp_a[0], dp_a[1], dp_a[2] ]
97 Create a array of the same posture Roll, Pitch, Yaw angles 98 from one set of Roll Pitch Yaw angles. 100 @type rpy : [float, float, float] 101 @param rpy : Posture angle Roll, Pitch, Yaw (r,p,y) [rad] to copy 102 @type pat_length : int 103 @param pat_length : Array length of pat_length for the same posture angles 105 @rtype : [ [float, float, float], [float, float, float], ...] 106 @return : Array of the same posture angles (r,p,y) [rad] 110 for i
in range(pat_length):
111 posture_rpy.append(rpy)
118 Create a array of the same time length 119 from whole motion time. 121 @type whole_tm : float 122 @param whole_tm : Whole time [s] 123 @type pat_length : int 124 @param pat_length : Number to divide the whole time 126 @rtype : [ float, float, float, ... ] 127 @return : Array of the equally divided whole time [s] 131 tm = whole_tm / pat_length
132 for i
in range(pat_length):
140 Create a array of limb joint angles for the each waypoints 141 from data of the end effector postions and postures 142 using robot moving using RTM interface. 145 @param limb : limb to create a pattern, right arm 'rarm' or left arm 'larm' 146 @type pos_list : [[float,float,float],[float,float,float],...] 147 @param pos_list : Array of end effector positions (x,y,z) [m] 148 @type rpy_list : [[float,float,float],[float,float,float],...] 149 @param rpy_list : Array of end effector postures (r,p,y) [m] 150 @type tm_list : [float,float,...] 151 @param tm_list : Array of motion time lengths of the each pose [s] 153 @rtype : [[float,float,...],[float,float,...],...] 154 @return : Array of limb joint angles [rad] 157 for pos, rpy, tm
in zip(pos_list, rpy_list, tm_list):
158 robot.setTargetPose(limb, pos, rpy, tm)
159 robot.waitInterpolationOfGroup(limb)
161 joint_angles_deg = robot.getJointAngles()[3:9]
163 joint_angles_deg = robot.getJointAngles()[9:]
164 joint_angles_rad = [math.radians(angle_in_degree)
for angle_in_degree
in joint_angles_deg]
165 pattern_arm.append(joint_angles_rad)
171 from nextage_ros_bridge
import nextage_client
173 if __name__ ==
'__main__':
175 Main sequence for moving at waypoints 178 robot = nextage_client.NextageClient()
206 positions_arm = rect_xyzs + circ_xyzs
207 rpys_arm = rect_rpys + circ_rpys
208 time_list = rect_time + circ_time
210 print(
'Limb: %s' % limb_name)
211 print(
'Positions (%d): %s' % (len(positions_arm), positions_arm))
212 print(
'RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))
213 print(
'Time List (%d): %s' % (len(time_list), time_list))
216 print(
'Generating Joint Angle Pattern')
218 play_pattern_time = time_list
221 print(
'Limb: %s' % limb_name)
222 print(
'Joint Angle Pattern [rad]: %s' % play_pattern_arm)
223 print(
'Motion Time Pattern [sec]: %s' % play_pattern_time)
228 print(
'Start: Play Pattern')
229 robot.playPatternOfGroup(limb_name, play_pattern_arm, play_pattern_time)
230 robot.waitInterpolationOfGroup(limb_name)
231 print(
'End: Play Pattern')
def equalTimeList(whole_tm, pat_length)
def rectangularPositions(dp_a=[0.25, dp_b=[0.45)
def setTargetPoseSequence(limb, pos_list, rpy_list, tm_list)
def samePostureRPY(rpy, pat_length)
def circularPositions(center=[0.3, radius=0.1, steps=12)