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


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