simple_arm_server.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004   simple_arm_server.py - this is a simple way to move an arm
00005   Copyright (c) 2011 Michael Ferguson.  All right reserved.
00006 
00007   Redistribution and use in source and binary forms, with or without
00008   modification, are permitted provided that the following conditions are met:
00009       * Redistributions of source code must retain the above copyright
00010         notice, this list of conditions and the following disclaimer.
00011       * Redistributions in binary form must reproduce the above copyright
00012         notice, this list of conditions and the following disclaimer in the
00013         documentation and/or other materials provided with the distribution.
00014       * Neither the name of the copyright holders nor the names of its 
00015         contributors may be used to endorse or promote products derived 
00016         from this software without specific prior written permission.
00017   
00018   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019   ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020   WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021   DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024   OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026   OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027   ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 """
00029 
00030 import roslib; roslib.load_manifest('simple_arm_server')
00031 import rospy, actionlib
00032 
00033 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00034 from control_msgs.msg import *
00035 
00036 import tf
00037 from tf.transformations import euler_from_quaternion, quaternion_from_euler
00038 
00039 from actionlib_msgs.msg import GoalStatus
00040 from kinematics_msgs.msg import KinematicSolverInfo, PositionIKRequest
00041 from kinematics_msgs.srv import GetKinematicSolverInfo, GetPositionIK, GetPositionIKRequest
00042 from geometry_msgs.msg import PoseStamped
00043 from std_msgs.msg import Float64
00044 from sensor_msgs.msg import JointState
00045 from simple_arm_server.msg import *
00046 from simple_arm_server.srv import *
00047 
00048 from math import *
00049 
00050 class SimpleArmServer:
00051     """ 
00052         A class to move a 4/5/6DOF arm to a designated location (x,y,z), 
00053         respecting wrist rotation (roll) and gripper rotation (pitch). 
00054     """
00055 
00056     def __init__(self):
00057         rospy.init_node("simple_arm_server")        
00058         
00059         # configuration
00060         self.dof = rospy.get_param("~arm_dof", 5)
00061         self.root = rospy.get_param("~root_name","base_link")
00062         self.tip = rospy.get_param("~tip_name","gripper_link")
00063         self.timeout = rospy.get_param("~timeout",5.0)
00064         self.offset = rospy.get_param("~offset",0.0)
00065 
00066         # connect to arm kinematics
00067         rospy.wait_for_service('arm_kinematics/get_ik')
00068         rospy.wait_for_service('arm_kinematics/get_ik_solver_info')
00069         self._get_ik_proxy = rospy.ServiceProxy('arm_kinematics/get_ik', GetPositionIK, persistent=True)
00070         self._get_ik_solver_info_proxy = rospy.ServiceProxy('arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
00071     
00072         # setup tf for translating poses
00073         self._listener = tf.TransformListener()
00074 
00075         # a publisher for arm movement
00076         self._client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
00077         self._client.wait_for_server()
00078         self._gripper = rospy.Publisher('gripper_controller/command', Float64)
00079 
00080         # listen to joint states
00081         self.servos = dict()
00082         rospy.Subscriber('joint_states', JointState, self.stateCb)
00083 
00084         # load action for moving arm
00085         self.server = actionlib.SimpleActionServer("move_arm", MoveArmAction, execute_cb=self.actionCb, auto_start=False)
00086         self.server.start()
00087 
00088         # service for checking trajectory
00089         rospy.Service('~get_trajectory_validity', GetTrajectoryValidity, self.validityCb)
00090 
00091         rospy.loginfo('simple_arm_server node started')
00092         rospy.spin()
00093 
00094 
00095     def validityCb(self, req):
00096         """ Check a trajectory for validity. """
00097         for action in req.motions:
00098             if action.type == ArmAction.MOVE_ARM:
00099                 if self.motionToTrajectory(action, req.header.frame_id) == None:
00100                     return GetTrajectoryValidityResponse(False)
00101         return GetTrajectoryValidityResponse(True)
00102 
00103 
00104     def stateCb(self, msg):
00105         """ Update our known location of joints, for interpolation. """
00106         for i in range(len(msg.name)):
00107             joint = msg.name[i]
00108             self.servos[joint] = msg.position[i]
00109 
00110 
00111     def actionCb(self, req):
00112         """ Given pose to move gripper to, do it. """ 
00113         self.arm_solver_info = self._get_ik_solver_info_proxy() 
00114         computed_actions = list()       
00115 
00116         # compute trajectories
00117         for action in req.motions:
00118             if self.server.is_preempt_requested():
00119                 self.server.set_preempted( MoveArmResult(False) )
00120                 return
00121             if action.type == ArmAction.MOVE_ARM:
00122                 msg = self.motionToTrajectory(action, req.header.frame_id)
00123                 if msg != None:
00124                     computed_actions.append( [ 'move', msg, msg.points[0].time_from_start ] )
00125                 else:
00126                     self.server.set_aborted( MoveArmResult(False) )
00127                     return
00128             else:
00129                 computed_actions.append( [ 'grip', Float64(action.command), action.move_time ] )
00130 
00131         # smooth and execute trajectories
00132         traj = None
00133         for action in computed_actions:
00134             msg = action[1]
00135             time = action[2]
00136             if action[0] == 'move':
00137                 # build trajectory
00138                 if traj == None:
00139                     traj = msg
00140                 else:
00141                     msg.points[0].time_from_start += traj.points[-1].time_from_start
00142                     traj.points.append(msg.points[0])
00143             else: 
00144                 # move arm
00145                 if traj != None:
00146                     if not self.executeTrajectory(traj):
00147                         return
00148                     traj = None
00149                 # move gripper
00150                 rospy.loginfo("Move gripper to " + str(msg.data))
00151                 self._gripper.publish( msg )
00152                 rospy.sleep( time )
00153         if traj != None:
00154             print traj
00155             if not self.executeTrajectory(traj):
00156                 return
00157 
00158         self.server.set_succeeded( MoveArmResult(True) )
00159         return
00160 
00161 
00162     def executeTrajectory(self, traj):
00163         """ Execute a trajectory. """
00164         goal = FollowJointTrajectoryGoal()
00165         goal.trajectory = traj
00166         traj.header.stamp = rospy.Time.now()
00167         rospy.loginfo("Sending action with " + str(len(traj.points)) + " points.")
00168         self._client.send_goal(goal)
00169         while self._client.get_state() != GoalStatus.SUCCEEDED:
00170             if self.server.is_preempt_requested():
00171                 self._client.cancel_goal()
00172                 self.server.set_preempted( MoveArmResult(False) )
00173                 return False
00174         rospy.loginfo("Action succeeded: " + str(self._client.wait_for_result()))   
00175         return True
00176    
00177 
00178     def motionToTrajectory(self, action, frame_id):
00179         """ Convert an arm movement action into a trajectory. """
00180         ps = PoseStamped()
00181         ps.header.frame_id = frame_id
00182         ps.pose = action.goal
00183         pose = self._listener.transformPose(self.root, ps)
00184         rospy.loginfo("Parsing move to:\n" + str(pose))
00185         
00186         # create IK request
00187         request = GetPositionIKRequest()
00188         request.timeout = rospy.Duration(self.timeout)
00189 
00190         request.ik_request.pose_stamped.header.frame_id = self.root;
00191         request.ik_request.ik_link_name = self.tip;
00192         request.ik_request.pose_stamped.pose.position.x = pose.pose.position.x
00193         request.ik_request.pose_stamped.pose.position.y = pose.pose.position.y
00194         request.ik_request.pose_stamped.pose.position.z = pose.pose.position.z
00195 
00196         e = euler_from_quaternion([pose.pose.orientation.x,pose.pose.orientation.y,pose.pose.orientation.z,pose.pose.orientation.w])
00197         e = [i for i in e]
00198         if self.dof < 6:
00199             # 5DOF, so yaw angle = atan2(Y,X-shoulder offset)
00200             e[2] = atan2(pose.pose.position.y, pose.pose.position.x-self.offset)
00201         if self.dof < 5:
00202             # 4 DOF, so yaw as above AND no roll
00203             e[0] = 0
00204         q =  quaternion_from_euler(e[0], e[1], e[2])
00205 
00206         request.ik_request.pose_stamped.pose.orientation.x = q[0]
00207         request.ik_request.pose_stamped.pose.orientation.y = q[1]
00208         request.ik_request.pose_stamped.pose.orientation.z = q[2]
00209         request.ik_request.pose_stamped.pose.orientation.w = q[3]
00210 
00211         request.ik_request.ik_seed_state.joint_state.name = self.arm_solver_info.kinematic_solver_info.joint_names
00212         request.ik_request.ik_seed_state.joint_state.position = [self.servos[joint] for joint in request.ik_request.ik_seed_state.joint_state.name]
00213 
00214         # get IK, wiggle if needed
00215         tries = 0
00216         pitch = e[1]
00217         print "roll", e[0]
00218         while tries < 80:
00219             try:
00220                 response = self._get_ik_proxy(request)
00221                 if response.error_code.val == response.error_code.SUCCESS:
00222                     break
00223                 else:
00224                     tries += 1
00225                     # wiggle gripper
00226                     pitch = e[1] + ((-1)**tries)*((tries+1)/2)*0.025
00227                     # update quaternion
00228                     q = quaternion_from_euler(e[0], pitch, e[2])
00229                     request.ik_request.pose_stamped.pose.orientation.x = q[0]
00230                     request.ik_request.pose_stamped.pose.orientation.y = q[1]
00231                     request.ik_request.pose_stamped.pose.orientation.z = q[2]
00232                     request.ik_request.pose_stamped.pose.orientation.w = q[3]
00233             except rospy.ServiceException, e:
00234                 print "Service did not process request: %s"%str(e)
00235 
00236         print response
00237         if response.error_code.val == response.error_code.SUCCESS:
00238             arm_solver_info = self._get_ik_solver_info_proxy()     
00239             msg = JointTrajectory()
00240             msg.joint_names = arm_solver_info.kinematic_solver_info.joint_names
00241             msg.points = list()
00242             point = JointTrajectoryPoint()
00243             point.positions = [ 0.0 for servo in msg.joint_names ]
00244             point.velocities = [ 0.0 for servo in msg.joint_names ]
00245             for joint in request.ik_request.ik_seed_state.joint_state.name:
00246                 i = msg.joint_names.index(joint)
00247                 point.positions[i] = response.solution.joint_state.position[response.solution.joint_state.name.index(joint)] 
00248             if action.move_time > rospy.Duration(0.0):
00249                 point.time_from_start = action.move_time
00250             else:
00251                 point.time_from_start = rospy.Duration(5.0)
00252             msg.points.append(point)
00253             msg.header.stamp = rospy.Time.now() + rospy.Duration(0.01)
00254 
00255             return msg
00256         else:
00257             return None
00258 
00259 
00260 if __name__ == '__main__':
00261     try:
00262         SimpleArmServer()
00263     except rospy.ROSInterruptException:
00264         rospy.loginfo("And that's all folks...")
00265 


simple_arm_server
Author(s): Michael Ferguson
autogenerated on Fri Jan 3 2014 11:58:44