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 import copy
00036 import math
00037 import socket
00038
00039 import actionlib
00040 import moveit_commander
00041 from moveit_commander import MoveGroupCommander, MoveItCommanderException, RobotCommander
00042 import rospy
00043 from rospy import ROSInitException
00044 from control_msgs.msg import FollowJointTrajectoryAction
00045 from control_msgs.msg import FollowJointTrajectoryGoal
00046 from geometry_msgs.msg import Pose
00047 from trajectory_msgs.msg import JointTrajectoryPoint
00048 from tf.transformations import quaternion_from_euler, euler_from_quaternion, compose_matrix, translation_from_matrix, euler_from_matrix
00049 import numpy
00050
00051 from hironx_ros_bridge.constant import Constant
00052
00053 _MSG_ASK_ISSUEREPORT = 'Your report to ' + \
00054 'https://github.com/start-jsk/rtmros_hironx/issues ' + \
00055 'about the issue you are seeing is appreciated.'
00056
00057
00058
00059 import atexit, os
00060 atexit.register(lambda : os._exit(0))
00061
00062 class ROS_Client(RobotCommander):
00063 '''
00064 This class holds methods that are specific to Kawada Industries' dual-arm
00065 robot called Hiro, via ROS.
00066
00067 @since: December 2014: Now this class is replacing the default programming interface
00068 with HIRONX (hironx_client.py).
00069 @since: July 2014: this class is only intended to be used through HIRONX
00070 class.
00071 '''
00072
00073 _MSG_NO_MOVEGROUP_FOUND = ('\nMake sure you\'ve launched MoveGroup ' +
00074 '(e.g. by launching ' +
00075 'moveit_planning_execution.launch)')
00076
00077 def __init__(self, jointgroups=None):
00078 '''
00079 @param jointgroups [str]: Deprecated. No need after version 1.1.4 onward.
00080 '''
00081
00082 try:
00083 rospy.get_master().getSystemState()
00084 except socket.error as e:
00085 errormsg = 'No ROS Master found. Without it, you cannot use ROS from this script, but you can still use RTM without issues. ' + \
00086 'To use ROS, do not forget to run rosbridge. How to do so? --> http://wiki.ros.org/rtmros_nextage/Tutorials/Operating%20Hiro%2C%20NEXTAGE%20OPEN'
00087 raise ROSInitException(errormsg)
00088 except Exception as e:
00089 errormsg = '[ros_client] Unknown exception occurred, so do not create ros client...'
00090 rospy.logerr(errormsg)
00091 raise e
00092
00093 if not rospy.has_param('robot_description'):
00094 rospy.logwarn('ROS Bridge is not started yet, Assuming you want just to use RTM')
00095 return
00096
00097 super(ROS_Client, self).__init__()
00098
00099 rospy.init_node('hironx_ros_client')
00100
00101
00102
00103 self._init_action_clients()
00104
00105 if not rospy.has_param('robot_description_semantic'):
00106 rospy.logwarn('Moveit is not started yet, if you want to use MoveIt!' + self._MSG_NO_MOVEGROUP_FOUND)
00107 return
00108
00109 try:
00110 self._init_moveit_commanders()
00111 except RuntimeError as e:
00112 rospy.logerr(str(e) + self._MSG_NO_MOVEGROUP_FOUND)
00113
00114
00115
00116
00117
00118
00119
00120 def _init_moveit_commanders(self):
00121 '''
00122 @raise RuntimeError: When MoveGroup is not running.
00123 '''
00124
00125 try:
00126 self.MG_LARM = self.get_group(Constant.GRNAME_LEFT_ARM_MOVEGROUP)
00127 self.MG_RARM = self.get_group(Constant.GRNAME_RIGHT_ARM_MOVEGROUP)
00128 self.MG_BOTHARMS = self.get_group(Constant.GRNAME_BOTH_ARMS)
00129 self.MG_HEAD = self.get_group(Constant.GRNAME_HEAD)
00130 self.MG_TORSO = self.get_group(Constant.GRNAME_TORSO)
00131 self.MG_UPPERBODY = self.get_group(Constant.GRNAME_UPPERBODY)
00132 self.MG_LARM.set_planner_id("RRTConnectkConfigDefault")
00133 self.MG_RARM.set_planner_id("RRTConnectkConfigDefault")
00134 self.MG_BOTHARMS.set_planner_id("RRTConnectkConfigDefault")
00135 self.MG_HEAD.set_planner_id("RRTConnectkConfigDefault")
00136 self.MG_TORSO.set_planner_id("RRTConnectkConfigDefault")
00137 self.MG_UPPERBODY.set_planner_id("RRTConnectkConfigDefault")
00138
00139
00140 self._movegr_larm_ref_frame = self.MG_LARM.get_pose_reference_frame()
00141 self._movegr_rarm_ref_frame = self.MG_RARM.get_pose_reference_frame()
00142 self._movegr_botharms_ref_frame = self.MG_BOTHARMS.get_pose_reference_frame()
00143 except RuntimeError as e:
00144 raise e
00145
00146 def _init_action_clients(self):
00147 '''
00148 This is only needed for accessing Actionlib clients directly, which
00149 is no longer needed for this class now that it inherits
00150 RobotCommander from MoveIt!. Still this line is kept for the methods
00151 deprecated but remain for backward compatibility.
00152 '''
00153 self._aclient_larm = actionlib.SimpleActionClient(
00154 '/larm_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
00155 self._aclient_rarm = actionlib.SimpleActionClient(
00156 '/rarm_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
00157 self._aclient_head = actionlib.SimpleActionClient(
00158 '/head_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
00159 self._aclient_torso = actionlib.SimpleActionClient(
00160 '/torso_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
00161
00162 self._aclient_larm.wait_for_server()
00163 self._goal_larm = FollowJointTrajectoryGoal()
00164 self._goal_larm.trajectory.joint_names.append("LARM_JOINT0")
00165 self._goal_larm.trajectory.joint_names.append("LARM_JOINT1")
00166 self._goal_larm.trajectory.joint_names.append("LARM_JOINT2")
00167 self._goal_larm.trajectory.joint_names.append("LARM_JOINT3")
00168 self._goal_larm.trajectory.joint_names.append("LARM_JOINT4")
00169 self._goal_larm.trajectory.joint_names.append("LARM_JOINT5")
00170
00171 self._aclient_rarm.wait_for_server()
00172 self._goal_rarm = FollowJointTrajectoryGoal()
00173 self._goal_rarm.trajectory.joint_names.append("RARM_JOINT0")
00174 self._goal_rarm.trajectory.joint_names.append("RARM_JOINT1")
00175 self._goal_rarm.trajectory.joint_names.append("RARM_JOINT2")
00176 self._goal_rarm.trajectory.joint_names.append("RARM_JOINT3")
00177 self._goal_rarm.trajectory.joint_names.append("RARM_JOINT4")
00178 self._goal_rarm.trajectory.joint_names.append("RARM_JOINT5")
00179
00180 self._aclient_head.wait_for_server()
00181 self._goal_head = FollowJointTrajectoryGoal()
00182 self._goal_head.trajectory.joint_names.append('HEAD_JOINT0')
00183 self._goal_head.trajectory.joint_names.append('HEAD_JOINT1')
00184
00185 self._aclient_torso.wait_for_server()
00186 self._goal_torso = FollowJointTrajectoryGoal()
00187 self._goal_torso.trajectory.joint_names.append('CHEST_JOINT0')
00188
00189 rospy.loginfo('Joint names; ' +
00190 'Torso: {}\n\tHead: {}\n\tL-Arm: {}\n\tR-Arm: {}'.format(
00191 self._goal_torso.trajectory.joint_names,
00192 self._goal_head.trajectory.joint_names,
00193 self._goal_larm.trajectory.joint_names,
00194 self._goal_rarm.trajectory.joint_names))
00195
00196 def go_init(self, init_pose_type=0, task_duration=7.0):
00197 '''
00198 Change the posture of the entire robot to the pre-defined init pose.
00199
00200 This method is equivalent to
00201 hironx_ros_bridge.hironx_client.HIRONX.goInitial method (https://github.com/start-jsk/rtmros_hironx/blob/83c3ff0ad2aabd8525631b08276d33b09c98b2bf/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py#L199),
00202 with an addition of utilizing MoveIt!, which means e.g. if there is
00203 possible perceived collision robots will take the path MoveIt! computes
00204 with collision avoidance taken into account.
00205
00206 @type task_duration: float
00207 @param init_pose_type:
00208 0: default init pose (specified as _InitialPose)
00209 1: factory init pose (specified as _InitialPose_Factory)
00210 '''
00211 rospy.loginfo('*** go_init begins ***')
00212 posetype_str = ''
00213 if 0 == init_pose_type:
00214 posetype_str = 'init_rtm'
00215 elif 1 == init_pose_type:
00216 posetype_str = 'init_rtm_factory'
00217 else:
00218 rospy.logerr("unsupporeted init_pose_type " + str(init_pose_type))
00219 self.MG_BOTHARMS.set_named_target(posetype_str)
00220 self.MG_BOTHARMS.go()
00221
00222 def go_offpose(self, task_duration=7.0):
00223 self.MG_UPPERBODY.set_named_target(Constant.POSE_OFF)
00224 self.MG_UPPERBODY.go()
00225
00226 def goInitial(self, init_pose_type=0, task_duration=7.0):
00227 '''
00228 This method internally calls self.go_init.
00229
00230 This method exists solely because of compatibility purpose with
00231 hironx_ros_bridge.hironx_client.HIRONX.goInitial, which
00232 holds a method "goInitial".
00233
00234 @param init_pose_type:
00235 0: default init pose (specified as _InitialPose)
00236 1: factory init pose (specified as _InitialPose_Factory)
00237 '''
00238 return self.go_init(init_pose_type, task_duration)
00239
00240 def set_joint_angles_rad(self, groupname, positions_radian, duration=7.0,
00241 wait=False):
00242 '''
00243 @deprecated: Use MoveitCommander.set_joint_value_target instead.
00244
00245 @type groupname: str
00246 @param groupname: This should exist in self.groupnames.
00247 @type positions_radian: [float]
00248 @type duration: float
00249 @type wait: bool
00250 '''
00251 if groupname == Constant.GRNAME_TORSO:
00252 action_client = self._aclient_torso
00253 goal = self._goal_torso
00254 elif groupname == Constant.GRNAME_HEAD:
00255 action_client = self._aclient_head
00256 goal = self._goal_head
00257 elif groupname == Constant.GRNAME_LEFT_ARM:
00258 action_client = self._aclient_larm
00259 goal = self._goal_larm
00260 elif groupname == Constant.GRNAME_RIGHT_ARM:
00261 action_client = self._aclient_rarm
00262 goal = self._goal_rarm
00263 else:
00264
00265 rospy.logerr('groupname {} not assigned'.format(groupname))
00266
00267 try:
00268 pt = JointTrajectoryPoint()
00269 pt.positions = positions_radian
00270 pt.time_from_start = rospy.Duration(duration)
00271 goal.trajectory.points = [pt]
00272 goal.trajectory.header.stamp = \
00273 rospy.Time.now() + rospy.Duration(duration)
00274
00275 action_client.send_goal(goal)
00276 if wait:
00277 rospy.loginfo(
00278 'wait_for_result for the joint group {} = {}'.format(
00279 groupname, action_client.wait_for_result()))
00280 except rospy.ROSInterruptException as e:
00281 rospy.loginfo(e.str())
00282
00283 def set_joint_angles_deg(self, groupname, positions_deg, duration=7.0,
00284 wait=False):
00285 '''
00286 @type groupname: str
00287 @param groupname: This should exist in self.groupnames.
00288 @type positions_deg: [float]
00289 @type duration: float
00290 @type wait: bool
00291 '''
00292 self.set_joint_angles_rad(groupname, self._to_rad_list(positions_deg),
00293 duration, wait)
00294
00295 def _to_rad_list(self, list_degree):
00296 '''
00297 @TODO Needs to be replaced by something more common, or at least moved
00298 somewhere more common.
00299
00300 @type list_degree: [float]
00301 @param list_degree: A list length of the number of joints.
00302 '''
00303 list_rad = []
00304 for deg in list_degree:
00305 rad = math.radians(deg)
00306 list_rad.append(rad)
00307 rospy.logdebug('Position deg={} rad={}'.format(deg, rad))
00308 return list_rad
00309
00310 def set_pose(self, joint_group, position, rpy=None, task_duration=7.0,
00311 do_wait=True, ref_frame_name=None):
00312 '''
00313 @deprecated: Use set_pose_target (from MoveGroupCommander) directly.
00314 Accept pose defined by position and RPY in Cartesian format.
00315
00316 @type joint_group: str
00317 @type position: [float]
00318 @param position: x, y, z.
00319 @type rpy: [float]
00320 @param rpy: If None, keep the current orientation by using
00321 MoveGroupCommander.set_position_target. See:
00322 'http://moveit.ros.org/doxygen/' +
00323 'classmoveit__commander_1_1move__group_1_1' +
00324 'MoveGroupCommander.html#acfe2220fd85eeb0a971c51353e437753'
00325 @param ref_frame_name: reference frame for target pose, i.e. "LARM_JOINT5_Link".
00326 '''
00327
00328 position = list(position)
00329 if not rpy is None:
00330 rpy = list(rpy)
00331
00332
00333 if not self.MG_LARM or not self.MG_RARM:
00334 try:
00335 self._init_moveit_commanders()
00336 except RuntimeError as e:
00337 rospy.logerr(self._MSG_NO_MOVEGROUP_FOUND)
00338 raise e
00339
00340
00341 movegr = None
00342 if Constant.GRNAME_LEFT_ARM == joint_group:
00343 movegr = self.MG_LARM
00344 elif Constant.GRNAME_RIGHT_ARM == joint_group:
00345 movegr = self.MG_RARM
00346 else:
00347 rospy.logerr('joint_group must be either %s, %s or %s'%(Constant.GRNAME_LEFT_ARM,
00348 Constant.GRNAME_RIGHT_ARM,
00349 Constant.GRNAME_BOTH_ARMS))
00350 return
00351
00352
00353 if ref_frame_name :
00354 ref_pose = movegr.get_current_pose(ref_frame_name).pose
00355 ref_mat = compose_matrix(
00356 translate = [ref_pose.position.x, ref_pose.position.y, ref_pose.position.z],
00357 angles = list(euler_from_quaternion([ref_pose.orientation.x,ref_pose.orientation.y,ref_pose.orientation.z,ref_pose.orientation.w])))
00358 target_mat = numpy.dot(ref_mat, compose_matrix(translate = position, angles = rpy or [0, 0, 0]))
00359 position = list(translation_from_matrix(target_mat))
00360 rpy = list(euler_from_matrix(target_mat))
00361
00362
00363 if not rpy:
00364 try:
00365 movegr.set_position_target(position)
00366 except MoveItCommanderException as e:
00367 rospy.logerr(str(e))
00368 (movegr.go(do_wait) or movegr.go(do_wait) or
00369 rospy.logerr('MoveGroup.go fails; jointgr={}'.format(joint_group)))
00370 return
00371
00372
00373
00374 pose = copy.deepcopy(position)
00375 pose.extend(rpy)
00376 rospy.loginfo('setpose jntgr={} mvgr={} pose={} posi={} rpy={}'.format(
00377 joint_group, movegr, pose, position, rpy))
00378 try:
00379 movegr.set_pose_target(pose)
00380 except MoveItCommanderException as e:
00381 rospy.logerr(str(e))
00382 except Exception as e:
00383 rospy.logerr(str(e))
00384
00385 (movegr.go(do_wait) or movegr.go(do_wait) or
00386 rospy.logerr('MoveGroup.go fails; jointgr={}'.format(joint_group)))