orrosplanning: ik_openrave.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # Copyright (c) 2010 Rosen Diankov (rosen.diankov@gmail.com)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #     http://www.apache.org/licenses/LICENSE-2.0
00009 # 
00010 # Unless required by applicable law or agreed to in writing, software
00011 # distributed under the License is distributed on an "AS IS" BASIS,
00012 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013 # See the License for the specific language governing permissions and
00014 # limitations under the License.
00015 from __future__ import with_statement # for python 2.5
00016 __author__ = 'Rosen Diankov'
00017 __copyright__ = 'Copyright (C) 2010 Rosen Diankov (rosen.diankov@gmail.com)'
00018 __license__ = 'Apache License, Version 2.0'
00019 import roslib; roslib.load_manifest('orrosplanning')
00020 import rospy
00021 import os
00022 
00023 from optparse import OptionParser
00024 from openravepy import *
00025 from openravepy.misc import OpenRAVEGlobalArguments
00026 from numpy import *
00027 import numpy,time,threading
00028 from itertools import izip
00029 import tf
00030 
00031 import orrosplanning.srv
00032 from orrosplanning.srv import IKRequest
00033 import sensor_msgs.msg
00034 import arm_navigation_msgs.msg
00035 from arm_navigation_msgs.msg import ArmNavigationErrorCodes
00036 import geometry_msgs.msg
00037 import trajectory_msgs.msg
00038 import kinematics_msgs.srv
00039 
00040 if __name__ == "__main__":
00041     parser = OptionParser(description='openrave planning example')
00042     OpenRAVEGlobalArguments.addOptions(parser)
00043     parser.add_option('--scene',action="store",type='string',dest='scene',default='robots/pr2-beta-static.zae',
00044                       help='scene to load (default=%default)')
00045     parser.add_option('--ipython', '-i',action="store_true",dest='ipython',default=False,
00046                       help='if true will drop into the ipython interpreter rather than spin')
00047     parser.add_option('--collision_map',action="store",type='string',dest='collision_map',default='/collision_map/collision_map',
00048                       help='The collision map topic (maping_msgs/CollisionMap), by (default=%default)')
00049     parser.add_option('--mapframe',action="store",type='string',dest='mapframe',default=None,
00050                       help='The frame of the map used to position the robot. If --mapframe="" is specified, then nothing will be transformed with tf')
00051     (options, args) = parser.parse_args()
00052     env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True)
00053     RaveLoadPlugin(os.path.join(roslib.packages.get_pkg_dir('orrosplanning'),'lib','liborrosplanning.so'))
00054     RaveLoadPlugin(os.path.join(roslib.packages.get_pkg_dir('openraveros'),'lib','openraveros'))
00055     namespace = 'openrave'
00056     env.AddModule(RaveCreateModule(env,'rosserver'),namespace)
00057     # load the orrosplanning plugin
00058     
00059     print 'initializing, please wait for ready signal...'
00060 
00061     try:
00062         rospy.init_node('ik_openrave',disable_signals=False)
00063         with env:
00064             env.Load(options.scene)
00065             robot = env.GetRobots()[0]
00066             # create ground right under the robot
00067             ab=robot.ComputeAABB()
00068             ground=RaveCreateKinBody(env,'')
00069             ground.SetName('map')
00070             ground.InitFromBoxes(array([r_[ab.pos()-array([0,0,ab.extents()[2]+0.002]),2.0,2.0,0.001]]),True)
00071             env.AddKinBody(ground,False)
00072             if options.mapframe is None:
00073                 options.mapframe = robot.GetLinks()[0].GetName()
00074                 rospy.loginfo('setting map frame to %s'%options.mapframe)
00075             collisionmap = RaveCreateSensorSystem(env,'CollisionMap bodyoffset %s topic %s'%(robot.GetName(),options.collision_map))
00076         
00077         valueslock = threading.Lock()
00078         listener = tf.TransformListener()
00079         values = robot.GetDOFValues()
00080         def UpdateRobotJoints(msg):
00081             with valueslock:
00082                 with env:
00083                     for name,pos in izip(msg.name,msg.position):
00084                         j = robot.GetJoint(name)
00085                         if j is not None:
00086                             values[j.GetDOFIndex()] = pos
00087                     robot.SetDOFValues(values)
00088 
00089         def IKFn(req):
00090             global options
00091             with valueslock:
00092                 with env:
00093                     if len(options.mapframe) > 0:
00094                         (robot_trans,robot_rot) = listener.lookupTransform(options.mapframe, robot.GetLinks()[0].GetName(), rospy.Time(0))
00095                         Trobot = matrixFromQuat([robot_rot[3],robot_rot[0],robot_rot[1],robot_rot[2]])
00096                         Trobot[0:3,3] = robot_trans
00097                         robot.SetTransform(Trobot)
00098                         goal = listener.transformPose(options.mapframe, req.pose_stamped)
00099                     else:
00100                         goal = req.pose_stamped
00101                     o = goal.pose.orientation
00102                     p = goal.pose.position
00103                     Thandgoal = matrixFromQuat([o.w,o.x,o.y,o.z])
00104                     Thandgoal[0:3,3] = [p.x,p.y,p.z]
00105 
00106                     if len(req.joint_state.name) > 0:
00107                         dofindices = [robot.GetJoint(name).GetDOFIndex() for name in req.joint_state.name]
00108                         robot.SetDOFValues(req.joint_state.position,dofindices)
00109 
00110                     res = orrosplanning.srv.IKResponse()
00111 
00112                     # resolve the ik type
00113                     iktype = None
00114                     if len(req.iktype) == 0:
00115                         iktype = IkParameterization.Type.Transform6D
00116                     else:
00117                         for value,type in IkParameterization.Type.values.iteritems():
00118                             if type.name.lower() == req.iktype.lower():
00119                                 iktype = type
00120                                 break
00121                         if iktype is None:
00122                             rospy.logerror('failed to find iktype %s'%(str(req.iktype)))
00123                             return None
00124 
00125                     ikp = IkParameterization()
00126                     if iktype == IkParameterization.Type.Direction3D:
00127                         ikp.SetDirection(Thandgoal[0:3,2])
00128                     elif iktype == IkParameterization.Type.Lookat3D:
00129                         ikp.SetLookat(Thandgoal[0:3,3])
00130                     elif iktype == IkParameterization.Type.Ray4D:
00131                         ikp.SetRay(Ray(Thandgoal[0:3,3],Thandgoal[0:3,2]))
00132                     elif iktype == IkParameterization.Type.Rotation3D:
00133                         ikp.SetRotation(quatFromRotationMatrix(Thandgoal[0:3,0:3]))
00134                     elif iktype == IkParameterization.Type.Transform6D:
00135                         ikp.SetTransform(Thandgoal)
00136                     elif iktype == IkParameterization.Type.Translation3D:
00137                         ikp.SetTranslation(Thandgoal[0:3,3])
00138                     elif iktype == IkParameterization.Type.TranslationDirection5D:
00139                         ikp.SetTranslationDirection5D(Ray(Thandgoal[0:3,3],Thandgoal[0:3,2]))
00140 
00141                     if len(req.manip_name) > 0:
00142                         manip = robot.GetManipulator(req.manip_name)
00143                         if manip is None:
00144                             rospy.logerror('failed to find manipulator %s'%req.manip_name)
00145                             return None
00146                     else:
00147                         manips = [manip for manip in robot.GetManipulators() if manip.GetEndEffector().GetName()==req.hand_frame_id]
00148                         if len(manips) == 0:
00149                             rospy.logerror('failed to find manipulator end effector %s'%req.hand_frame_id)
00150                             return None
00151                         manip = manips[0]
00152 
00153                     if manip.CheckIndependentCollision():
00154                         res.error_code.val = ArmNavigationErrorCodes.START_STATE_IN_COLLISION
00155                         return
00156 
00157                     robot.SetActiveManipulator(manip)
00158                     robot.SetActiveDOFs(manip.GetArmIndices())
00159                     
00160                     if manip.GetIkSolver() is None or not manip.GetIkSolver().Supports(IkParameterization.Type.TranslationDirection5D):
00161                         rospy.loginfo('generating ik for %s'%str(manip))
00162                         ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=iktype)
00163                         if not ikmodel.load():
00164                             ikmodel.autogenerate()
00165 
00166                     if req.filteroptions & IKRequest.IGNORE_ENVIRONMENT_COLLISIONS:
00167                         filteroptions = 0
00168                     else:
00169                         filteroptions = IkFilterOptions.CheckEnvCollisions
00170                     if req.filteroptions & IKRequest.IGNORE_SELF_COLLISIONS:
00171                         filteroptions |= IkFilterOptions.IgnoreSelfCollisions
00172                     if req.filteroptions & IKRequest.IGNORE_JOINT_LIMITS:
00173                         filteroptions |= IkFilterOptions.IgnoreJointLimits
00174 
00175                     if req.filteroptions & IKRequest.RETURN_ALL_SOLUTIONS:
00176                         solutions = manip.FindIKSolutions(ikp,filteroptions)
00177                     elif req.filteroptions & IKRequest.RETURN_CLOSEST_SOLUTION:
00178                         solutions = manip.FindIKSolutions(ikp,filteroptions)
00179                         if solutions is not None and len(solutions) > 0:
00180                             # find closest configuration space distance with respect to source solution
00181                             weights = robot.GetActiveDOFWeights()
00182                             dists = []
00183                             for sol in solutions:
00184                                 diff = robot.SubtractActiveDOFValues(sol,robot.GetActiveDOFValues())
00185                                 dists.append(dot(weights,diff**2))
00186                             index = argmin(dists)
00187                             solutions = [solutions[index]]
00188                     else:
00189                         solution = manip.FindIKSolution(ikp,filteroptions)
00190                         if solution is not None:
00191                             solutions = [solution]
00192                         else:
00193                             solutions = []
00194 
00195                     if len(solutions) == 0:
00196                         res.error_code.val = ArmNavigationErrorCodes.NO_IK_SOLUTION
00197                     else:
00198                         res.error_code.val = ArmNavigationErrorCodes.SUCCESS
00199                         res.solutions.header.stamp = rospy.Time.now()
00200                         res.solutions.joint_names = [j.GetName() for j in robot.GetJoints(manip.GetArmIndices())]
00201                         for s in solutions:
00202                             res.solutions.points.append(trajectory_msgs.msg.JointTrajectoryPoint(positions=s))
00203                     return res
00204 
00205         def GetPositionIKFn(reqall):
00206             global options
00207             req=reqall.ik_request
00208             with valueslock:
00209                 with env:
00210                     if len(options.mapframe) > 0:
00211                         (robot_trans,robot_rot) = listener.lookupTransform(options.mapframe, robot.GetLinks()[0].GetName(), rospy.Time(0))
00212                         Trobot = matrixFromQuat([robot_rot[3],robot_rot[0],robot_rot[1],robot_rot[2]])
00213                         Trobot[0:3,3] = robot_trans
00214                         robot.SetTransform(Trobot)
00215                         goal = listener.transformPose(options.mapframe, req.pose_stamped)
00216                     else:
00217                         goal = req.pose_stamped
00218                     o = goal.pose.orientation
00219                     p = goal.pose.position
00220                     Thandgoal = matrixFromQuat([o.w,o.x,o.y,o.z])
00221                     Thandgoal[0:3,3] = [p.x,p.y,p.z]
00222 
00223                     if len(req.robot_state.joint_state.name) > 0:
00224                         dofindices = [robot.GetJoint(name).GetDOFIndex() for name in req.robot_state.joint_state.name]
00225                         robot.SetDOFValues(req.robot_state.joint_state.position,dofindices)
00226                     #if len(req.robot_state.multi_dof_joint_state) > 0:
00227                     #    rospy.logwarn('ik_openrave.py does not support multi_dof_joint_state yet')
00228 
00229                     manips = [manip for manip in robot.GetManipulators() if manip.GetEndEffector().GetName()==req.ik_link_name]
00230                     if len(manips) == 0:
00231                         rospy.logerr('failed to find manipulator end effector %s'%req.ik_link_name)
00232                         return None
00233                     manip = manips[0]
00234                     rospy.logdebug('ik_openrave.py choosing %s manipulator'%manip.GetName())
00235                     robot.SetActiveManipulator(manip)
00236 
00237                     if manip.GetIkSolver() is None:
00238                         rospy.loginfo('generating ik for %s'%str(manip))
00239                         ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
00240                         if not ikmodel.load():
00241                             ikmodel.autogenerate()
00242 
00243 
00244                     res = kinematics_msgs.srv.GetPositionIKResponse()
00245                     if manip.CheckIndependentCollision():
00246                         res.error_code.val = ArmNavigationErrorCodes.START_STATE_IN_COLLISION
00247                         return
00248 
00249                     filteroptions = IkFilterOptions.CheckEnvCollisions
00250                     solution = manip.FindIKSolution(Thandgoal,filteroptions)
00251                     if solution is None:
00252                         res.error_code.val = ArmNavigationErrorCodes.NO_IK_SOLUTION
00253                     else:
00254                         res.solution.joint_state.header.stamp = rospy.Time.now()
00255                         res.solution.joint_state.name = [j.GetName() for j in robot.GetJoints(manip.GetArmIndices())]
00256                         res.solution.joint_state.position = list(solution)
00257                     return res
00258 
00259         sub = rospy.Subscriber("/joint_states", sensor_msgs.msg.JointState, UpdateRobotJoints,queue_size=1)
00260         s1 = rospy.Service('IK', orrosplanning.srv.IK, IKFn)
00261         s2 = rospy.Service('GetPositionIK', kinematics_msgs.srv.GetPositionIK, GetPositionIKFn)
00262         rospy.loginfo('openrave services ready: %s, %s'%(s1.resolved_name,s2.resolved_name))
00263 
00264         if options.ipython:
00265             from IPython.Shell import IPShellEmbed
00266             ipshell = IPShellEmbed(argv='',banner = 'Dropping into IPython',exit_msg = 'Leaving Interpreter, back to program.')
00267             ipshell(local_ns=locals())
00268         else:
00269             rospy.spin()
00270     finally:
00271         RaveDestroy()