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)
