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
00039 import unittest
00040
00041 from geometry_msgs.msg import Pose
00042 from moveit_commander import MoveGroupCommander, MoveItCommanderException, RobotCommander
00043 from moveit_msgs.msg import RobotTrajectory
00044 import rospy
00045
00046 import math
00047 from tf.transformations import quaternion_from_euler
00048
00049 class TestDualarmMoveit(unittest.TestCase):
00050 _KINEMATICSOLVER_SAFE = 'RRTConnectkConfigDefault'
00051
00052 _MOVEGROUP_NAME_TORSO = 'torso'
00053 _JOINTNAMES_TORSO = ['CHEST_JOINT0']
00054
00055 _MOVEGROUP_ATTR_TORSO = [_MOVEGROUP_NAME_TORSO, _JOINTNAMES_TORSO]
00056
00057 _MOVEGROUP_NAME_LEFTARM = 'left_arm'
00058 _JOINTNAMES_LEFTARM = ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']
00059 _MOVEGROUP_ATTR_LEFTARM = [_MOVEGROUP_NAME_LEFTARM, _JOINTNAMES_LEFTARM]
00060
00061 _MOVEGROUP_NAME_RIGHTARM = 'right_arm'
00062 _JOINTNAMES_RIGHTARM = ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
00063 _MOVEGROUP_ATTR_RIGHTARM = [_MOVEGROUP_NAME_RIGHTARM, _JOINTNAMES_RIGHTARM]
00064
00065 _MOVEGROUP_NAME_LEFTARM_TORSO = 'left_arm_torso'
00066 _JOINTNAMES_LEFTARM_TORSO = _JOINTNAMES_TORSO +_JOINTNAMES_LEFTARM
00067 _MOVEGROUP_ATTR_LEFTARM_TORSO = [_MOVEGROUP_NAME_LEFTARM_TORSO, _JOINTNAMES_LEFTARM_TORSO]
00068
00069 _MOVEGROUP_NAME_RIGHTARM_TORSO = 'right_arm_torso'
00070 _JOINTNAMES_RIGHTARM_TORSO = _JOINTNAMES_TORSO + _JOINTNAMES_RIGHTARM
00071 _MOVEGROUP_ATTR_RIGHTARM_TORSO = [_MOVEGROUP_NAME_RIGHTARM_TORSO, _JOINTNAMES_RIGHTARM_TORSO]
00072
00073 _MOVEGROUP_NAME_LEFTHAND = 'left_hand'
00074 _JOINTNAMES_LEFTHAND = ['LARM_JOINT5']
00075 _MOVEGROUP_ATTR_LEFTHAND = [_MOVEGROUP_NAME_LEFTHAND, _JOINTNAMES_LEFTHAND]
00076
00077 _MOVEGROUP_NAME_RIGHTHAND = 'right_hand'
00078 _JOINTNAMES_RIGHTHAND = ['RARM_JOINT5']
00079 _MOVEGROUP_ATTR_RIGHTHAND = [_MOVEGROUP_NAME_RIGHTHAND, _JOINTNAMES_RIGHTHAND]
00080
00081 _MOVEGROUP_NAME_HEAD = 'head'
00082 _JOINTNAMES_HEAD = ['HEAD_JOINT0', 'HEAD_JOINT1']
00083 _MOVEGROUP_ATTR_HEAD = [_MOVEGROUP_NAME_HEAD, _JOINTNAMES_HEAD]
00084
00085 _MOVEGROUP_NAME_BOTHARMS = 'botharms'
00086
00087 _JOINTNAMES_BOTHARMS = _JOINTNAMES_LEFTARM + _JOINTNAMES_RIGHTARM
00088 _MOVEGROUP_ATTR_BOTHARMS = [_MOVEGROUP_NAME_BOTHARMS, _JOINTNAMES_BOTHARMS]
00089
00090 _MOVEGROUP_NAME_UPPERBODY = 'upperbody'
00091 _JOINTNAMES_UPPERBODY = _JOINTNAMES_TORSO + _JOINTNAMES_HEAD + _JOINTNAMES_LEFTARM + _JOINTNAMES_RIGHTARM
00092 _MOVEGROUP_ATTR_UPPERBODY = [_MOVEGROUP_NAME_UPPERBODY, _JOINTNAMES_UPPERBODY]
00093
00094
00095 _MOVEGROUP_NAMES = sorted([_MOVEGROUP_NAME_TORSO, _MOVEGROUP_NAME_HEAD,
00096 _MOVEGROUP_NAME_LEFTARM, _MOVEGROUP_NAME_RIGHTARM,
00097 _MOVEGROUP_NAME_LEFTARM_TORSO, _MOVEGROUP_NAME_RIGHTARM_TORSO,
00098 _MOVEGROUP_NAME_LEFTHAND, _MOVEGROUP_NAME_RIGHTHAND,
00099 _MOVEGROUP_NAME_BOTHARMS, _MOVEGROUP_NAME_UPPERBODY])
00100 _MOVEGROUP_ATTRS = [_MOVEGROUP_ATTR_TORSO, _MOVEGROUP_ATTR_HEAD,
00101 _MOVEGROUP_ATTR_LEFTARM, _MOVEGROUP_ATTR_RIGHTARM,
00102 _MOVEGROUP_ATTR_LEFTARM_TORSO, _MOVEGROUP_ATTR_RIGHTARM_TORSO,
00103 _MOVEGROUP_ATTR_LEFTHAND, _MOVEGROUP_ATTR_RIGHTHAND,
00104 _MOVEGROUP_ATTR_BOTHARMS, _MOVEGROUP_ATTR_UPPERBODY]
00105
00106 _TARGETPOSE_LEFT = []
00107
00108
00109
00110
00111
00112
00113
00114
00115 @classmethod
00116 def setUpClass(self):
00117
00118 rospy.init_node('test_moveit')
00119 rospy.sleep(5)
00120
00121 self.robot = RobotCommander()
00122
00123
00124
00125 for mg_attr in self._MOVEGROUP_ATTRS:
00126 mg = MoveGroupCommander(mg_attr[0])
00127
00128 mg.set_planner_id(self._KINEMATICSOLVER_SAFE)
00129
00130 mg.allow_replanning(True)
00131
00132 mg.set_planning_time(30.0)
00133
00134 mg_attr.append(mg)
00135
00136 @classmethod
00137 def tearDownClass(self):
00138 True
00139
00140 def _set_sample_pose(self):
00141 '''
00142 @return: Pose() with some values populated in.
00143 '''
00144 pose_target = Pose()
00145 pose_target.orientation.x = 0.00
00146 pose_target.orientation.y = 0.00
00147 pose_target.orientation.z = -0.20
00148 pose_target.orientation.w = 0.98
00149 pose_target.position.x = 0.18
00150 pose_target.position.y = -0.00
00151 pose_target.position.z = 0.94
00152 return pose_target
00153
00154 def _plan(self):
00155 '''
00156 Run `clear_pose_targets` at the beginning.
00157 @rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
00158 '''
00159 self._mvgroup.clear_pose_targets()
00160
00161 pose_target = self._set_sample_pose()
00162 self._mvgroup.set_pose_target(pose_target)
00163 plan = self._mvgroup.plan()
00164 rospy.loginfo(' plan: {}'.format(plan))
00165 return plan
00166
00167 def test_list_movegroups(self):
00168 '''Check if the defined move groups are loaded.'''
00169 self.assertEqual(self._MOVEGROUP_NAMES, sorted(self.robot.get_group_names()))
00170
00171 def test_list_activejoints(self):
00172 '''Check if the defined joints in a move group are loaded.'''
00173 for mg_attr in self._MOVEGROUP_ATTRS:
00174 self.assertEqual(mg_attr[1],
00175 sorted(mg_attr[2].get_active_joints()))
00176
00177 def _plan_leftarm(self, movegroup):
00178 '''
00179 @type movegroup: MoveGroupCommander
00180 @rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
00181 '''
00182 movegroup.clear_pose_targets()
00183
00184 pose_target = Pose()
00185 pose_target.orientation.x = -0.32136357
00186 pose_target.orientation.y = -0.63049522
00187 pose_target.orientation.z = 0.3206799
00188 pose_target.orientation.w = 0.62957575
00189 pose_target.position.x = 0.32529
00190 pose_target.position.y = 0.29919
00191 pose_target.position.z = 0.24389
00192
00193 movegroup.set_pose_target(pose_target)
00194 plan = movegroup.plan()
00195 rospy.loginfo('Plan: {}'.format(plan))
00196 return plan
00197
00198 def test_plan_success(self):
00199 '''Evaluate plan (RobotTrajectory)'''
00200 plan = self._plan_leftarm(self._MOVEGROUP_ATTR_LEFTARM[2])
00201
00202
00203
00204 self.assertNotEqual(0, plan.joint_trajectory.points)
00205
00206 def test_planandexecute(self):
00207 '''
00208 Evaluate Plan and Execute works.
00209 Currently no value checking is done (, which is better to be implemented)
00210 '''
00211 mvgroup = self._MOVEGROUP_ATTR_LEFTARM[2]
00212 self._plan_leftarm(mvgroup)
00213
00214
00215 self.assertTrue(mvgroup.go() or mvgroup.go() or mvgroup.go())
00216
00217 def test_left_and_right_plan(self):
00218 '''
00219 Check if http://wiki.ros.org/rtmros_nextage/Tutorials/Programming_Hiro_NEXTAGE_OPEN_MOVEIT works
00220 '''
00221 larm = self._MOVEGROUP_ATTR_LEFTARM[2]
00222 rarm = self._MOVEGROUP_ATTR_RIGHTARM[2]
00223
00224
00225 rarm_initial_pose = rarm.get_current_pose().pose
00226 print "=" * 10, " Printing Right Hand initial pose: "
00227 print rarm_initial_pose
00228
00229
00230 larm_initial_pose = larm.get_current_pose().pose
00231 print "=" * 10, " Printing Left Hand initial pose: "
00232 print larm_initial_pose
00233
00234 target_pose_r = Pose()
00235 target_pose_r.position.x = 0.325471850974-0.01
00236 target_pose_r.position.y = -0.182271241593-0.3
00237 target_pose_r.position.z = 0.0676272396419+0.3
00238 target_pose_r.orientation.x = -0.000556712307053
00239 target_pose_r.orientation.y = -0.706576742941
00240 target_pose_r.orientation.z = -0.00102461782513
00241 target_pose_r.orientation.w = 0.707635461636
00242 rarm.set_pose_target(target_pose_r)
00243
00244 print "=" * 10," plan1 ..."
00245 self.assertTrue(rarm.go() or rarm.go() or rarm.go())
00246 rospy.sleep(1)
00247
00248 target_pose_l = [
00249 target_pose_r.position.x,
00250 -target_pose_r.position.y,
00251 target_pose_r.position.z,
00252 target_pose_r.orientation.x,
00253 target_pose_r.orientation.y,
00254 target_pose_r.orientation.z,
00255 target_pose_r.orientation.w
00256 ]
00257 larm.set_pose_target(target_pose_l)
00258
00259 print "=" * 10," plan2 ..."
00260 self.assertTrue(larm.go() or larm.go() or larm.go())
00261 rospy.sleep(1)
00262
00263
00264 rarm.clear_pose_targets()
00265
00266
00267 target_pose_r.position.x = 0.221486843301
00268 target_pose_r.position.y = -0.0746407547512
00269 target_pose_r.position.z = 0.642545484602
00270 target_pose_r.orientation.x = 0.0669013615474
00271 target_pose_r.orientation.y = -0.993519060661
00272 target_pose_r.orientation.z = 0.00834224628291
00273 target_pose_r.orientation.w = 0.0915122442864
00274 rarm.set_pose_target(target_pose_r)
00275
00276 print "=" * 10, " plan3..."
00277 self.assertTrue(rarm.go() or rarm.go() or rarm.go())
00278 rospy.sleep(1)
00279
00280 print "=" * 10,"Initial pose ..."
00281 rarm.set_pose_target(rarm_initial_pose)
00282 larm.set_pose_target(larm_initial_pose)
00283 self.assertTrue(rarm.go() or rarm.go() or rarm.go())
00284 self.assertTrue(larm.go() or larm.go() or larm.go())
00285
00286 def test_botharms_plan(self):
00287 botharms = self._MOVEGROUP_ATTR_BOTHARMS[2]
00288
00289 target_pose_r = Pose()
00290 target_pose_l = Pose()
00291 q = quaternion_from_euler(0, -math.pi/2,0)
00292 target_pose_r.position.x = 0.3
00293 target_pose_r.position.y = 0.1
00294 target_pose_r.position.z = 0.0
00295 target_pose_r.orientation.x = q[0]
00296 target_pose_r.orientation.y = q[1]
00297 target_pose_r.orientation.z = q[2]
00298 target_pose_r.orientation.w = q[3]
00299 target_pose_l.position.x = 0.3
00300 target_pose_l.position.y =-0.1
00301 target_pose_l.position.z = 0.3
00302 target_pose_l.orientation.x = q[0]
00303 target_pose_l.orientation.y = q[1]
00304 target_pose_l.orientation.z = q[2]
00305 target_pose_l.orientation.w = q[3]
00306 botharms.set_pose_target(target_pose_r, 'RARM_JOINT5_Link')
00307 botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
00308 self.assertTrue(botharms.go() or botharms.go() or botharms.go())
00309 rospy.sleep(1)
00310
00311 target_pose_r.position.x = 0.3
00312 target_pose_r.position.y =-0.2
00313 target_pose_r.position.z = 0.0
00314 target_pose_l.position.x = 0.3
00315 target_pose_l.position.y = 0.2
00316 target_pose_l.position.z = 0.0
00317 botharms.set_pose_target(target_pose_r, 'RARM_JOINT5_Link')
00318 botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
00319 self.assertTrue(botharms.go() or botharms.go() or botharms.go())
00320
00321 if __name__ == '__main__':
00322 import rostest
00323 rostest.rosrun('nextage_ros_bridge', 'test_moveit', TestDualarmMoveit)