def motionToTrajectory(self, action, frame_id):
""" Convert an arm movement action into a trajectory. """
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = action.goal
pose = self._listener.transformPose(self.root, ps)
rospy.loginfo("Parsing move to:\n" + str(pose))
# create IK request
request = GetConstraintAwarePositionIKRequest()
request.timeout = rospy.Duration(self.timeout)
request.ik_request.pose_stamped.header.frame_id = self.root;
request.ik_request.ik_link_name = self.tip;
request.ik_request.pose_stamped.pose.position.x = pose.pose.position.x
request.ik_request.pose_stamped.pose.position.y = pose.pose.position.y
request.ik_request.pose_stamped.pose.position.z = pose.pose.position.z
e = euler_from_quaternion([pose.pose.orientation.x,pose.pose.orientation.y,pose.pose.orientation.z,pose.pose.orientation.w])
e = [i for i in e]
if self.dof < 6:
# 5DOF, so yaw angle = atan2(Y,X-shoulder offset)
e[2] = atan2(pose.pose.position.y, pose.pose.position.x)
if self.dof < 5:
# 4 DOF, so yaw as above AND no roll
e[0] = 0
q = quaternion_from_euler(e[0], e[1], e[2])
request.ik_request.pose_stamped.pose.orientation.x = q[0]
request.ik_request.pose_stamped.pose.orientation.y = q[1]
request.ik_request.pose_stamped.pose.orientation.z = q[2]
request.ik_request.pose_stamped.pose.orientation.w = q[3]
request.ik_request.ik_seed_state.joint_state.name = self.arm_solver_info.kinematic_solver_info.joint_names
request.ik_request.ik_seed_state.joint_state.position = [self.servos[joint] for joint in request.ik_request.ik_seed_state.joint_state.name]
# get IK, wiggle if needed
tries = 0
pitch = e[1]
while tries < 80:
try:
response = self._get_ik_proxy(request)
if response.error_code.val == response.error_code.SUCCESS:
break
else:
tries += 1
# wiggle gripper
pitch = e[1] + ((-1)**tries)*((tries+1)/2)*0.025
# update quaternion
q = quaternion_from_euler(e[0], pitch, e[2])
request.ik_request.pose_stamped.pose.orientation.x = q[0]
request.ik_request.pose_stamped.pose.orientation.y = q[1]
request.ik_request.pose_stamped.pose.orientation.z = q[2]
request.ik_request.pose_stamped.pose.orientation.w = q[3]
except rospy.ServiceException, e:
rospy.logerr("Service did not process request: %s"%str(e))
def form_constraint_aware_ik_request(self, ps):
#print "forming IK request for :%s" %ps
req = GetConstraintAwarePositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = \
self.ik_info.kinematic_solver_info.link_names[-1]
req.ik_request.ik_seed_state.joint_state.name = \
self.ik_info.kinematic_solver_info.joint_names
req.ik_request.ik_seed_state.joint_state.position = \
self.joint_state_pos
return req
def form_constraint_aware_ik_request(self, ps):
#print "forming IK request for :%s" %ps
req = GetConstraintAwarePositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = \
self.ik_info.kinematic_solver_info.link_names[-1]
req.ik_request.ik_seed_state.joint_state.name = \
self.ik_info.kinematic_solver_info.joint_names
req.ik_request.ik_seed_state.joint_state.position = \
self.joint_state_pos
return req
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
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
