def ik_request(self, pose,
end_point='right_hand', joint_seed=None,
nullspace_goal=None, nullspace_gain=0.4,
allow_collision=False):
"""
Inverse Kinematics request sent to IK Service
@type pose: geometry_msgs.Pose
@param pose: Cartesian pose of the end point
@type end_point: string
@param end_point: name of the end point (should be in URDF)
@type joint_seed: dict({str:float})
@param joint_seed: the joint angles for the initial IK guess (optional)
@type nullspace_goal: dict({str:float})
@param nullspace_goal: desired joints, or subset of joints, to bias the solver (optional)
@type nullspace_gain: double
@param nullspace_gain: gain used to bias toward the nullspace goal [0.0, 1.0] (optional)
@type allow_collision: bool
@param allow_collision: does not check if Ik solution is in collision
@rtype: dict({str:float})
@return: valid joint positions if exists. False if no solution is found.
"""
if not isinstance(pose, Pose):
rospy.logerr('pose is not of type geometry_msgs.msgs.Pose')
return False
# Add desired pose for inverse kinematics
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
ikreq.tip_names.append(end_point)
# The joint seed is where the IK position solver starts its optimization
if joint_seed is not None:
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = list(joint_seed.keys())
seed.position = list(joint_seed.values())
ikreq.seed_angles.append(seed)
# Once the primary IK task is solved, the solver will then try to bias the
# the joint angles toward the goal joint configuration. The null space is
# the extra degrees of freedom the joints can move without affecting the
# primary IK task.
if nullspace_goal is not None:
ikreq.use_nullspace_goal.append(True)
# The nullspace goal can either be the full set or subset of joint angles
goal = JointState()
goal.names = list(nullspace_goal.keys())
goal.position = list(nullspace_goal.values())
ikreq.nullspace_goal.append(goal)
# The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
# If empty, the default gain of 0.4 will be used
ikreq.nullspace_gain.append(nullspace_gain)
try:
resp = self._iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("IK Service call failed: %s" % (e,))
return False
limb_joints = {}
# Check if result valid, and type of seed ultimately used to get solution
if (resp.result_type[0] > 0
or (allow_collision and resp.result_type[0] == resp.IK_IN_COLLISION)):
# Format solution into Limb API-compatible dictionary
limb_joints = dict(list(zip(resp.joints[0].name, resp.joints[0].position)))
else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
return False
return limb_joints
