00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 import rospy
00039 import geometry_msgs.msg
00040 import copy
00041 import tf
00042
00043 from moveit_commander import MoveGroupCommander
00044
00045 import nextage_rtm_playpattern as nxtpp
00046
00047
00048 def setTargetPoseSequenceMoveIt(limb, pos_list, rpy_list, tm_list):
00049 '''
00050 Create a array of limb joint angles for the each waypoints
00051 from data of the end effector postions and postures
00052 using MoveIt!.
00053
00054 @type limb : str
00055 @param limb : limb to create a pattern, right arm 'rarm' or left arm 'larm'
00056 @type pos_list : [[float,float,float],[float,float,float],...]
00057 @param pos_list : Array of end effector positions (x,y,z) [m]
00058 @type rpy_list : [[float,float,float],[float,float,float],...]
00059 @param rpy_list : Array of end effector postures (r,p,y) [m]
00060 @type tm_list : [float,float,...]
00061 @param tm_list : Array of motion time lengths of the each pose [s]
00062
00063 @rtype : RobotTrajectory, float
00064 @return : Plan of limb waypoints motion and fraction of the path achieved as described by the waypoints
00065 '''
00066 wpose = geometry_msgs.msg.Pose()
00067 waypoints = []
00068
00069 for pos, rpy, tm in zip(pos_list, rpy_list, tm_list):
00070 quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
00071 wpose.position.x = pos[0]
00072 wpose.position.y = pos[1]
00073 wpose.position.z = pos[2]
00074 wpose.orientation.x = quaternion[0]
00075 wpose.orientation.y = quaternion[1]
00076 wpose.orientation.z = quaternion[2]
00077 wpose.orientation.w = quaternion[3]
00078 waypoints.append(copy.deepcopy(wpose))
00079
00080 (pln, frc) = limb.compute_cartesian_path(
00081 waypoints,
00082 0.01,
00083 0.0)
00084
00085 return pln, frc
00086
00087
00088 if __name__ == '__main__':
00089 '''
00090 Main sequence for moving at waypoints
00091 using MoveIt!.
00092 '''
00093 rospy.init_node('commander_example', anonymous=True)
00094
00095 limb_name = "left_arm"
00096 group = MoveGroupCommander(limb_name)
00097
00098 rarm_initial_pose = group.get_current_pose().pose
00099 rospy.loginfo("Got Initial Pose \n{}".format(rarm_initial_pose))
00100
00101 pose_target = geometry_msgs.msg.Pose()
00102 pose_target = copy.deepcopy(rarm_initial_pose)
00103
00104 positions_arm = []
00105 rpys_arm = []
00106 time_list = []
00107
00108
00109 rect_xyzs = nxtpp.rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1])
00110 rect_rpys = nxtpp.samePostureRPY([-3.073437, -1.569023, 3.073247], len(rect_xyzs))
00111 rect_time = nxtpp.equalTimeList(10.0, len(rect_xyzs))
00112
00113
00114 circ_xyzs = nxtpp.circularPositions(center=[0.35, 0.1, 0.1], radius=0.1 ,steps=12)
00115 circ_rpys = nxtpp.samePostureRPY([-3.073437, -1.569023, 3.073247], len(circ_xyzs))
00116 circ_time = nxtpp.equalTimeList(10.0, len(circ_xyzs))
00117
00118
00119 rect_time[0] += 1.0
00120 circ_time[0] += 1.0
00121
00122
00123 positions_arm = rect_xyzs + circ_xyzs
00124 rpys_arm = rect_rpys + circ_rpys
00125 time_list = rect_time + circ_time
00126
00127 print('Limb: %s' % limb_name)
00128 print('Positions (%d): %s' % (len(positions_arm), positions_arm))
00129 print('RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))
00130 print('Time List (%d): %s' % (len(time_list), time_list))
00131
00132
00133 rospy.loginfo("Planing Waypoints")
00134 plan, flaction = setTargetPoseSequenceMoveIt(group, positions_arm, rpys_arm, time_list)
00135
00136
00137 rospy.loginfo("Waiting while the plan is visualized on RViz")
00138 rospy.sleep(20)
00139
00140
00141 rospy.loginfo("Execute!!")
00142 group.execute(plan)
00143 rospy.loginfo("Executiion Done")
00144 rospy.sleep(5)
00145
00146
00147 rospy.loginfo("Move to Initial Pose")
00148 pose_target = copy.deepcopy(rarm_initial_pose)
00149 rospy.loginfo("set target to {}".format(pose_target))
00150 group.set_pose_target(pose_target)
00151 plan = group.plan()
00152 ret = group.go()