def _updateIKFeasibility(self, ps):
p = PoseStamped()
p.pose.orientation = ps.pose.orientation
p.header.frame_id = '/torso_lift_link'
offset = -0.216 #distance from tool frame to wrist_roll_link
q = ps.pose.orientation
rot = tr.tft.quaternion_matrix((q.x, q.y, q.z, q.w))
offset_vec = rot * np.matrix([[offset, 0., 0., 1.]]).T
p.pose.position.x = ps.pose.position.x + offset_vec[0]
p.pose.position.y = ps.pose.position.y + offset_vec[1]
p.pose.position.z = ps.pose.position.z + offset_vec[2]
self.test_pos_pub.publish(p)
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
ik_req = PositionIKRequest()
ik_req.ik_link_name = self.ik_solver_info.link_names[-1]
ik_req.ik_seed_state.joint_state.name = self.ik_solver_info.joint_names
ik_req.ik_seed_state.joint_state.position = self.joint_state
ik_req.pose_stamped = ps
req.ik_request = ik_req
ik_sol = self.pr2_ik_client.call(req)
if ik_sol.error_code.val < 0:
print "IK Failed with error code: %s" %ik_sol.error_code.val
for marker in self.wp_im.controls[0].markers:
marker.color = ColorRGBA(1,0,0,0.7)
else:
for marker in self.wp_im.controls[0].markers:
marker.color = ColorRGBA(0,1,0,0.7)
def get_ik(self, pose_stamped, collision_aware=True, starting_state=None,
seed_state=None, timeout=5.0, ordered_collisions=None):
'''
Solves inverse kinematics problems.
**Args:**
**pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions
*collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world
*starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
matching this state. If no state is passed in, it will use the current state in the planning scene
interface.
*seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK. If no state is passed it, it
will use the current state in planning scene interface.
*timeout (double):* Time in seconds that IK is allowed to run
**Returns:**
A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False. The robot state returned has
the arm joints set to the IK solution if found and all other joints set to that of starting_state.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls an IK service which may use TF for transformations! Probably best to only use with pose stampeds
defined in the robot frame (convert them yourself using the planning scene interface).
'''
rospy.logdebug('Solving IK for\n'+str(pose_stamped))
pos_req = PositionIKRequest()
pos_req.ik_link_name = self.hand.hand_frame
pos_req.pose_stamped = pose_stamped
if not starting_state:
starting_state = self._psi.get_robot_state()
if not seed_state:
seed_state = self.arm_robot_state(starting_state)
pos_req.ik_seed_state = seed_state
pos_req.robot_state = starting_state
if collision_aware:
self._psi.add_ordered_collisions(ordered_collisions)
coll_req = GetConstraintAwarePositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._collision_aware_ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(coll_res.solution.joint_state,
copy.deepcopy(starting_state))
self._psi.remove_ordered_collisions(ordered_collisions)
return coll_res
coll_req = GetPositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(coll_res.solution.joint_state,
copy.deepcopy(starting_state))
return coll_res
def run_ik(
self,
pose_stamped,
start_angles,
link_name,
collision_aware=1,
ordered_collision_operations=None,
IK_robot_state=None,
link_padding=None,
):
if link_name not in self.link_names:
rospy.logerr("link name %s not possible!" % link_name)
return None
self.link_name = link_name
# print "request:\n", pose_stamped
ik_request = PositionIKRequest()
ik_request.ik_link_name = self.link_name
ik_request.pose_stamped = pose_stamped
ik_request.ik_seed_state.joint_state.header.stamp = rospy.get_rostime()
ik_request.ik_seed_state.joint_state.position = start_angles
ik_request.ik_seed_state.joint_state.name = self.joint_names
if IK_robot_state:
ik_request.robot_state = IK_robot_state
try:
if collision_aware and self.perception_running:
col_free_ik_request = GetConstraintAwarePositionIKRequest()
col_free_ik_request.ik_request = ik_request
col_free_ik_request.timeout = rospy.Duration(10.0) # timeout after 10 seconds
if ordered_collision_operations != None:
col_free_ik_request.ordered_collision_operations = ordered_collision_operations
if link_padding != None:
col_free_ik_request.link_padding = link_padding
resp = self._ik_service_with_collision(col_free_ik_request)
else:
resp = self._ik_service(ik_request, rospy.Duration(10.0))
except rospy.ServiceException, e:
rospy.logwarn("IK service call failed! error msg: %s" % e)
return (None, None)
def run_ik(self,
pose_stamped,
start_angles,
link_name,
collision_aware=1,
ordered_collision_operations=None,
IK_robot_state=None,
link_padding=None):
if link_name not in self.link_names:
rospy.logerr("link name %s not possible!" % link_name)
return None
self.link_name = link_name
#print "request:\n", pose_stamped
ik_request = PositionIKRequest()
ik_request.ik_link_name = self.link_name
ik_request.pose_stamped = pose_stamped
ik_request.ik_seed_state.joint_state.header.stamp = rospy.get_rostime()
ik_request.ik_seed_state.joint_state.position = start_angles
ik_request.ik_seed_state.joint_state.name = self.joint_names
if IK_robot_state:
ik_request.robot_state = IK_robot_state
try:
if collision_aware and self.perception_running:
col_free_ik_request = GetConstraintAwarePositionIKRequest()
col_free_ik_request.ik_request = ik_request
col_free_ik_request.timeout = rospy.Duration(
10.0) #timeout after 10 seconds
if ordered_collision_operations != None:
col_free_ik_request.ordered_collision_operations = ordered_collision_operations
if link_padding != None:
col_free_ik_request.link_padding = link_padding
resp = self._ik_service_with_collision(col_free_ik_request)
else:
resp = self._ik_service(ik_request, rospy.Duration(10.0))
except rospy.ServiceException, e:
rospy.logwarn("IK service call failed! error msg: %s" % e)
return (None, None)
def get_ik(self,
pose_stamped,
collision_aware=True,
starting_state=None,
seed_state=None,
timeout=5.0,
ordered_collisions=None):
'''
Solves inverse kinematics problems.
**Args:**
**pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions
*collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world
*starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
matching this state. If no state is passed in, it will use the current state in the planning scene
interface.
*seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK. If no state is passed it, it
will use the current state in planning scene interface.
*timeout (double):* Time in seconds that IK is allowed to run
**Returns:**
A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False. The robot state returned has
the arm joints set to the IK solution if found and all other joints set to that of starting_state.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls an IK service which may use TF for transformations! Probably best to only use with pose stampeds
defined in the robot frame (convert them yourself using the planning scene interface).
'''
rospy.logdebug('Solving IK for\n' + str(pose_stamped))
pos_req = PositionIKRequest()
pos_req.ik_link_name = self.hand.hand_frame
pos_req.pose_stamped = pose_stamped
if not starting_state:
starting_state = self._psi.get_robot_state()
if not seed_state:
seed_state = self.arm_robot_state(starting_state)
pos_req.ik_seed_state = seed_state
pos_req.robot_state = starting_state
if collision_aware:
self._psi.add_ordered_collisions(ordered_collisions)
coll_req = GetConstraintAwarePositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._collision_aware_ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(
coll_res.solution.joint_state, copy.deepcopy(starting_state))
self._psi.remove_ordered_collisions(ordered_collisions)
return coll_res
coll_req = GetPositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(
coll_res.solution.joint_state, copy.deepcopy(starting_state))
return coll_res