def find_ik_for_grasping_pose(self, pose_stamped):
solver_info = self.get_solver_info_srv()
arm_joints = solver_info.kinematic_solver_info.joint_names
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = GRIPPER_LINK_FRAME
req.ik_request.pose_stamped = pose_stamped
try:
current_state = rospy.wait_for_message('/joint_states', JointState, 2.0)
req.ik_request.ik_seed_state.joint_state.name = arm_joints
req.ik_request.ik_seed_state.joint_state.position = [current_state.position[current_state.name.index(j)] for j in arm_joints]
if not self.set_planning_scene_diff_client():
rospy.logerr('%s: Find IK for Grasp: failed to set planning scene diff' % ACTION_NAME)
return None
ik_result = self.get_ik_srv(req)
if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
return ik_result.solution
else:
rospy.logerr('%s: failed to find an IK for requested grasping pose, aborting' % ACTION_NAME)
return None
except:
rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME)
return None
def set_left_arm_pose(self, position, orientation):
'''
Move the left arm to a position
@param position: x,y,z
@param angles: quaternion
'''
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = "l_wrist_roll_link"
req.ik_request.pose_stamped.header.frame_id = "base_link"
req.ik_request.pose_stamped.pose.position.x = position[0]
req.ik_request.pose_stamped.pose.position.y = position[1]
req.ik_request.pose_stamped.pose.position.z = position[2]
req.ik_request.pose_stamped.pose.orientation.x = orientation[0]
req.ik_request.pose_stamped.pose.orientation.y = orientation[1]
req.ik_request.pose_stamped.pose.orientation.z = orientation[2]
req.ik_request.pose_stamped.pose.orientation.w = orientation[3]
req.ik_request.ik_seed_state.joint_state.position = self.robot_state.left_arm_positions
req.ik_request.ik_seed_state.joint_state.name = self.robot_state.left_joint_names
res = self.left_ik(req)
if res.error_code.val != 1:
rospy.logerr("IK error, code is %d" % res.error_code.val)
return None
joints = res.solution.joint_state.position
return joints
def set_left_arm_pose(self, position, orientation):
'''
Move the left arm to a position
@param position: x,y,z
@param angles: quaternion
'''
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = "l_wrist_roll_link"
req.ik_request.pose_stamped.header.frame_id = "base_link"
req.ik_request.pose_stamped.pose.position.x = position[0]
req.ik_request.pose_stamped.pose.position.y = position[1]
req.ik_request.pose_stamped.pose.position.z = position[2]
req.ik_request.pose_stamped.pose.orientation.x = orientation[0]
req.ik_request.pose_stamped.pose.orientation.y = orientation[1]
req.ik_request.pose_stamped.pose.orientation.z = orientation[2]
req.ik_request.pose_stamped.pose.orientation.w = orientation[3]
req.ik_request.ik_seed_state.joint_state.position = self.robot_state.left_arm_positions
req.ik_request.ik_seed_state.joint_state.name = self.robot_state.left_joint_names
res = self.left_ik(req)
if res.error_code.val != 1:
rospy.logerr("IK error, code is %d" % res.error_code.val)
return None
joints = res.solution.joint_state.position
return joints
def IK(self, arm, p, rot, q_guess):
rospy.logwarn('Currently ignoring the arm parameter.')
ik_req = GetPositionIKRequest()
ik_req.timeout = rospy.Duration(5.)
ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
ik_req.ik_request.pose_stamped.pose.position.x = p[0, 0]
ik_req.ik_request.pose_stamped.pose.position.y = p[1, 0]
ik_req.ik_request.pose_stamped.pose.position.z = p[2, 0]
quat = tr.matrix_to_quaternion(rot)
ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]
ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list
ik_resp = self.ik_srv.call(ik_req)
if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
ret = ik_resp.solution.joint_state.position
else:
rospy.logerr('Inverse kinematics failed')
ret = None
return ret
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 IK(self, arm, p, rot, q_guess):
rospy.logwarn('Currently ignoring the arm parameter.')
ik_req = GetPositionIKRequest()
ik_req.timeout = rospy.Duration(5.)
ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
ik_req.ik_request.pose_stamped.pose.position.x = p[0,0]
ik_req.ik_request.pose_stamped.pose.position.y = p[1,0]
ik_req.ik_request.pose_stamped.pose.position.z = p[2,0]
quat = tr.matrix_to_quaternion(rot)
ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]
ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list
ik_resp = self.ik_srv.call(ik_req)
if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
ret = ik_resp.solution.joint_state.position
else:
rospy.logerr('Inverse kinematics failed')
ret = None
return ret
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 = GetPositionIKRequest()
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-self.offset)
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]
print "roll", e[0]
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:
print "Service did not process request: %s"%str(e)
def form_ik_request(self, ps):
#print "forming IK request for :%s" %ps
req = GetPositionIKRequest()
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_act.positions
return req
def form_ik_request(self, ps):
#print "forming IK request for :%s" %ps
req = GetPositionIKRequest()
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_act.positions
return req
def form_ik_request(self, ps):
"""Compose an IK request from ik solver info and pose goal"""
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.ik_link_name = self.ik_info.link_names[-1]
req.ik_request.pose_stamped = ps
req.ik_request.ik_seed_state.joint_state.name = self.ik_info.joint_names
with self.joint_state_lock:
positions = self.joint_state['positions'][:]
velocities = self.joint_state['velocities'][:]
req.ik_request.ik_seed_state.joint_state.position = positions
req.ik_request.ik_seed_state.joint_state.velocity = velocities
return req
def form_ik_request(self, ps):
"""Compose an IK request from ik solver info and pose goal"""
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.ik_link_name = self.ik_info.link_names[-1]
req.ik_request.pose_stamped = ps
req.ik_request.ik_seed_state.joint_state.name = self.ik_info.joint_names
with self.joint_state_lock:
positions = self.joint_state['positions'][:]
velocities = self.joint_state['velocities'][:]
req.ik_request.ik_seed_state.joint_state.position = positions
req.ik_request.ik_seed_state.joint_state.velocity = velocities
return req
def form_ik_request(self, ps):
#print "Form IK request for: \r\n" %str(ps)
if (self.joint_state):
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = self.curr_pose.fk_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
#print "IK Request: %s" %str(ik_request)
return req
else:
rospy.loginfo("No Joint States Available Yet")
def form_ik_request(self, ps):
#print "Form IK request for: \r\n" %str(ps)
if (self.joint_state):
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = self.curr_pose.fk_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
#print "IK Request: %s" %str(ik_request)
return req
else:
rospy.loginfo("No Joint States Available Yet")
def IK(self, pose, q_guess=[0]*7):
ik_req = GetPositionIKRequest()
ik_req.timeout = rospy.Duration(5.)
ik_req.ik_request.ik_link_name = self.arm + '_wrist_roll_link'
pos = pose[:3, 3].T.A[0].tolist()
quat = tf_trans.quaternion_from_matrix(pose)
ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
ik_req.ik_request.pose_stamped.pose = Pose(Point(*pos), Quaternion(*quat))
ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list
ik_resp = self.ik_srv.call(ik_req)
if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
return ik_resp.solution.joint_state.position
return None
def IK(self, arm, pos, rot, q_guess=[0]*7):
ik_req = GetPositionIKRequest()
ik_req.timeout = rospy.Duration(5.)
if arm == 0:
ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
else:
ik_req.ik_request.ik_link_name = 'l_wrist_roll_link'
ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
quat = tr.matrix_to_quaternion(rot)
ik_req.ik_request.pose_stamped.pose = Pose(Point(*pos), Quaternion(*quat))
ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list[arm]
ik_resp = self.ik_srv[arm].call(ik_req)
return ik_resp.solution.joint_state.position
def IK(self, pose, q_guess=[0] * 7):
ik_req = GetPositionIKRequest()
ik_req.timeout = rospy.Duration(5.)
ik_req.ik_request.ik_link_name = self.arm + '_wrist_roll_link'
pos = pose[:3, 3].T.A[0].tolist()
quat = tf_trans.quaternion_from_matrix(pose)
ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
ik_req.ik_request.pose_stamped.pose = Pose(Point(*pos),
Quaternion(*quat))
ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list
ik_resp = self.ik_srv.call(ik_req)
if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
return ik_resp.solution.joint_state.position
return None
def _setup_ik(self):
'''Sets up services for inverse kinematics'''
ik_info_srv_name = ('pr2_' + self._side() +
'_arm_kinematics_simple/get_ik_solver_info')
ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
rospy.wait_for_service(ik_info_srv_name)
ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
GetKinematicSolverInfo)
solver_info = ik_info_srv()
rospy.loginfo('IK info service has responded for ' + self._side() +
' arm.')
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK,
persistent=True)
rospy.loginfo('IK service has responded for ' + self._side() + ' arm.')
# Set up common parts of an IK request
self.ik_request = GetPositionIKRequest()
self.ik_request.timeout = rospy.Duration(4.0)
self.ik_joints = solver_info.kinematic_solver_info.joint_names
self.ik_limits = solver_info.kinematic_solver_info.limits
ik_links = solver_info.kinematic_solver_info.link_names
request = self.ik_request.ik_request
request.ik_link_name = ik_links[0]
request.pose_stamped.header.frame_id = 'base_link'
request.ik_seed_state.joint_state.name = self.ik_joints
request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints)
def _setup_ik(self):
'''Sets up services for inverse kinematics.'''
side = self._side()
# Get IK info service.
ik_info_srv_name = PR2_SERVICE_PREFIX + side + SERVICE_IK_INFO_POSTFIX
rospy.wait_for_service(ik_info_srv_name)
ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
GetKinematicSolverInfo)
solver_info = ik_info_srv()
rospy.loginfo('IK info service has responded for ' + side + ' arm.')
# Get IK service.
ik_srv_name = PR2_SERVICE_PREFIX + side + SERVICE_IK_POSTFIX
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK,
persistent=True)
rospy.loginfo('IK service has responded for ' + side + ' arm.')
# Set up common parts of an IK request.
self.ik_request = GetPositionIKRequest()
self.ik_request.timeout = rospy.Duration(IK_REQUEST_TIMEOUT)
self.ik_joints = solver_info.kinematic_solver_info.joint_names
self.ik_limits = solver_info.kinematic_solver_info.limits
ik_links = solver_info.kinematic_solver_info.link_names
request = self.ik_request.ik_request
request.ik_link_name = ik_links[0]
request.pose_stamped.header.frame_id = BASE_LINK
request.ik_seed_state.joint_state.name = self.ik_joints
request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints)
def __init__(self, side_prefix):
self.side_prefix = side_prefix
# Set up Inversse Kinematics services
ik_info_srv_name = ('pr2_' + self._side() +
'_arm_kinematics_simple/get_ik_solver_info')
ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
rospy.loginfo('Waiting for IK info service to respond.')
rospy.wait_for_service(ik_info_srv_name)
ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
GetKinematicSolverInfo)
solver_info = ik_info_srv()
self.ik_joints = solver_info.kinematic_solver_info.joint_names
self.ik_limits = solver_info.kinematic_solver_info.limits
rospy.loginfo('Waiting for IK service to respond.')
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK,
persistent=True)
rospy.loginfo('IK initialized for arm: ' + self._side())
# Set up common parts of an IK request
self.ik_request = GetPositionIKRequest()
self.ik_request.timeout = rospy.Duration(4.0)
self.ik_request.ik_request.ik_link_name = solver_info.kinematic_solver_info.link_names[
0]
self.ik_request.ik_request.pose_stamped.header.frame_id = 'base_link'
self.ik_request.ik_request.ik_seed_state.joint_state.name = self.ik_joints
self.ik_request.ik_request.ik_seed_state.joint_state.position = [
0
] * len(self.ik_joints)
def IK(self, arm, p, rot, q_guess):
if arm != 1:
arm = 0
p = make_column(p)
if rot.shape == (3, 3):
quat = np.matrix(tr.matrix_to_quaternion(rot)).T
elif rot.shape == (4, 1):
quat = make_column(rot)
else:
rospy.logerr('Inverse kinematics failed (bad rotation)')
return None
# Transform point back to wrist roll link
neg_off = [-self.off_point[0], -self.off_point[1], -self.off_point[2]]
transpos = self.transform_in_frame(p, quat, neg_off)
ik_req = GetPositionIKRequest()
ik_req.timeout = rospy.Duration(5.)
if arm == 0:
ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
else:
ik_req.ik_request.ik_link_name = 'l_wrist_roll_link'
ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
ik_req.ik_request.pose_stamped.pose.position.x = transpos[0]
ik_req.ik_request.pose_stamped.pose.position.y = transpos[1]
ik_req.ik_request.pose_stamped.pose.position.z = transpos[2]
ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]
ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list[
arm]
ik_resp = self.ik_srv[arm].call(ik_req)
if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
ret = list(ik_resp.solution.joint_state.position)
else:
rospy.logerr('Inverse kinematics failed')
ret = None
return ret
def IK(self, arm, p, rot, q_guess):
if arm != 1:
arm = 0
p = make_column(p)
if rot.shape == (3, 3):
quat = np.matrix(tr.matrix_to_quaternion(rot)).T
elif rot.shape == (4, 1):
quat = make_column(rot)
else:
rospy.logerr('Inverse kinematics failed (bad rotation)')
return None
# Transform point back to wrist roll link
neg_off = [-self.off_point[0],-self.off_point[1],-self.off_point[2]]
transpos = self.transform_in_frame(p, quat, neg_off)
ik_req = GetPositionIKRequest()
ik_req.timeout = rospy.Duration(5.)
if arm == 0:
ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
else:
ik_req.ik_request.ik_link_name = 'l_wrist_roll_link'
ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
ik_req.ik_request.pose_stamped.pose.position.x = transpos[0]
ik_req.ik_request.pose_stamped.pose.position.y = transpos[1]
ik_req.ik_request.pose_stamped.pose.position.z = transpos[2]
ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]
ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list[arm]
ik_resp = self.ik_srv[arm].call(ik_req)
if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
ret = list(ik_resp.solution.joint_state.position)
else:
rospy.logerr('Inverse kinematics failed')
ret = None
return ret
def solve_ik(x, y, z):
'''
Need to call move_arm_ompl before calling this, otherwise it doesn't work.
'''
joint_names = [ "r_shoulder_pan_joint",
"r_shoulder_lift_joint",
"r_upper_arm_roll_joint",
"r_elbow_flex_joint",
"r_forearm_roll_joint",
"r_wrist_flex_joint",
"r_wrist_roll_joint"]
service_name = "pr2_right_arm_kinematics/get_ik"
rospy.wait_for_service(service_name)
try:
ik_solver = rospy.ServiceProxy(service_name, GetPositionIK)
req = GetPositionIKRequest()
req.timeout = roslib.rostime.Duration(5)
req.ik_request.ik_link_name = "r_wrist_roll_link"
req.ik_request.pose_stamped.header.frame_id = "base_link"
req.ik_request.pose_stamped.pose.position.x = x
req.ik_request.pose_stamped.pose.position.y = y
req.ik_request.pose_stamped.pose.position.z = z
req.ik_request.pose_stamped.pose.orientation.x = 0.0
req.ik_request.pose_stamped.pose.orientation.y = 0.0
req.ik_request.pose_stamped.pose.orientation.z = 0.0
req.ik_request.pose_stamped.pose.orientation.w = 0.0
req.ik_request.ik_seed_state.joint_state.position = [-0.5, -0.18, -2, -1.2, 5, -1.6, 0]
req.ik_request.ik_seed_state.joint_state.name = joint_names
resp = GetPositionIKResponse()
resp = ik_solver(req)
print resp.solution.joint_state.name
print resp.solution.joint_state.position
if len(resp.solution.joint_state.position) == 7:
move_right_arm(resp.solution.joint_state.position)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def solve_ik(x, y, z):
'''
Need to call move_arm_ompl before calling this, otherwise it doesn't work.
'''
joint_names = [
"r_shoulder_pan_joint", "r_shoulder_lift_joint",
"r_upper_arm_roll_joint", "r_elbow_flex_joint", "r_forearm_roll_joint",
"r_wrist_flex_joint", "r_wrist_roll_joint"
]
service_name = "pr2_right_arm_kinematics/get_ik"
rospy.wait_for_service(service_name)
try:
ik_solver = rospy.ServiceProxy(service_name, GetPositionIK)
req = GetPositionIKRequest()
req.timeout = roslib.rostime.Duration(5)
req.ik_request.ik_link_name = "r_wrist_roll_link"
req.ik_request.pose_stamped.header.frame_id = "base_link"
req.ik_request.pose_stamped.pose.position.x = x
req.ik_request.pose_stamped.pose.position.y = y
req.ik_request.pose_stamped.pose.position.z = z
req.ik_request.pose_stamped.pose.orientation.x = 0.0
req.ik_request.pose_stamped.pose.orientation.y = 0.0
req.ik_request.pose_stamped.pose.orientation.z = 0.0
req.ik_request.pose_stamped.pose.orientation.w = 0.0
req.ik_request.ik_seed_state.joint_state.position = [
-0.5, -0.18, -2, -1.2, 5, -1.6, 0
]
req.ik_request.ik_seed_state.joint_state.name = joint_names
resp = GetPositionIKResponse()
resp = ik_solver(req)
print resp.solution.joint_state.name
print resp.solution.joint_state.position
if len(resp.solution.joint_state.position) == 7:
move_right_arm(resp.solution.joint_state.position)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def lookat(pose, torso_ik):
'''
query arm_ik server for joint_position which put arm_7_link to pose
@param arm_ik: arm_ik service proxy
@param t: translation
@param q: rotation as quaternion
@param link: frame in which pose (t, q) is defined
@param seed: initial joint positions for ik calculation, in None current joint pos of arm is used.
@return: tuple of joint_positions or None if ik was not found
'''
# get joint names for arm from parameter server
t=pose[0]
q = pose[1]
# create and send ik request
req = GetPositionIKRequest()
req.timeout = rospy.Duration(1.0)
req.ik_request.ik_link_name = "lookat_focus_frame"
req.ik_request.pose_stamped.header.frame_id = 'base_link'
req.ik_request.pose_stamped.pose.position.x = t[0]
req.ik_request.pose_stamped.pose.position.y = t[1]
req.ik_request.pose_stamped.pose.position.z = t[2]
req.ik_request.pose_stamped.pose.orientation.x = q[0]
req.ik_request.pose_stamped.pose.orientation.y = q[1]
req.ik_request.pose_stamped.pose.orientation.z = q[2]
req.ik_request.pose_stamped.pose.orientation.w = q[3]
# try to get inverse kinecmatics for at least 3 times
for i in range(3):
resp = torso_ik(req)
if resp.error_code.val == resp.error_code.SUCCESS:
break
# report sucess or return None on error
if resp.error_code.val == resp.error_code.SUCCESS:
result = list(resp.solution.joint_state.position)
return result
def __init__(self):
rospy.init_node("pi_robot_get_ik")
a=1
self.joint_state_pub = rospy.Publisher("joint_states", JointState)
self.joint_state_update = JointState()
self.joint_state_update = self.get_joints()
self.rate = 1
r = rospy.Rate(self.rate)
rospy.wait_for_service('test_arm_kinematics/get_ik')
rospy.wait_for_service('test_arm_kinematics/get_ik_solver_info')
get_ik_proxy = rospy.ServiceProxy('test_arm_kinematics/get_ik', GetPositionIK, persistent=True)
get_ik_solver_info_proxy = rospy.ServiceProxy('test_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
left_arm_solver_info = get_ik_solver_info_proxy()
self.request = GetPositionIKRequest()
self.request.timeout = rospy.Duration(5.0)
self.request.ik_request.pose_stamped.header.frame_id = "base_actuator";
self.request.ik_request.ik_link_name = "gripper_base";
self.request.ik_request.pose_stamped.pose.position.x = 0.366796872165
self.request.ik_request.pose_stamped.pose.position.y = 0.0149175972575
self.request.ik_request.pose_stamped.pose.position.z = 0.282845877564
self.request.ik_request.pose_stamped.pose.orientation.x = 0.159293225427
self.request.ik_request.pose_stamped.pose.orientation.y = 0.898180025306
self.request.ik_request.pose_stamped.pose.orientation.z = 0.0735797897789
self.request.ik_request.pose_stamped.pose.orientation.w = -0.403093444514
self.request.ik_request.ik_seed_state.joint_state.name = left_arm_solver_info.kinematic_solver_info.joint_names
self.request.ik_request.ik_seed_state.joint_state.position = [0]*len(self.request.ik_request.ik_seed_state.joint_state.name )
while not rospy.is_shutdown():
try:
self.response = get_ik_proxy(self.request)
print self.response.error_code
# if( self.response.error_code ==1 ):#== ik_response.error_code.SUCCESS :
# print "ok"
rospy.loginfo(self.response)
for joint in self.request.ik_request.ik_seed_state.joint_state.name:
self.joint_state_update.position[self.joint_state_update.name.index(joint)] = self.response.solution.joint_state.position[self.response.solution.joint_state.name.index(joint)]
self.joint_state_update.header.stamp = rospy.Time.now()
self.joint_state_pub.publish(self.joint_state_update)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
r.sleep()
def find_ik_for_grasping_pose(self, pose_stamped):
solver_info = self.get_solver_info_srv()
arm_joints = solver_info.kinematic_solver_info.joint_names
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = GRIPPER_LINK_FRAME
req.ik_request.pose_stamped = pose_stamped
try:
current_state = rospy.wait_for_message('/joint_states', JointState,
2.0)
req.ik_request.ik_seed_state.joint_state.name = arm_joints
req.ik_request.ik_seed_state.joint_state.position = [
current_state.position[current_state.name.index(j)]
for j in arm_joints
]
if not self.set_planning_scene_diff_client():
rospy.logerr(
'%s: Find IK for Grasp: failed to set planning scene diff'
% ACTION_NAME)
return None
ik_result = self.get_ik_srv(req)
if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
return ik_result.solution
else:
rospy.logerr(
'%s: failed to find an IK for requested grasping pose, aborting'
% ACTION_NAME)
return None
except:
rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME)
return None
def find_ik_for_grasping_pose(pose_stamped):
rospy.wait_for_service('wubble_left_arm_kinematics/get_ik_solver_info')
get_solver_info_srv = rospy.ServiceProxy('/wubble_left_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
rospy.loginfo('connected to wubble_left_arm_kinematics/get_ik_solver_info service')
solver_info = get_solver_info_srv()
arm_joints = solver_info.kinematic_solver_info.joint_names
rospy.wait_for_service('wubble_left_arm_kinematics/get_ik')
get_ik_srv = rospy.ServiceProxy('/wubble_left_arm_kinematics/get_ik', GetPositionIK)
rospy.loginfo('connected to wubble_left_arm_kinematics/get_ik service')
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = 'L7_wrist_roll_link';
req.ik_request.pose_stamped = pose_stamped
current_state = rospy.wait_for_message('/joint_states', JointState)
rospy.loginfo('recevied current joint states')
req.ik_request.ik_seed_state.joint_state.name = arm_joints
req.ik_request.ik_seed_state.joint_state.position = [current_state.position[current_state.name.index(j)] for j in arm_joints]
rospy.loginfo('current joint positions are')
rospy.loginfo('joint names: %s' % str(req.ik_request.ik_seed_state.joint_state.name))
rospy.loginfo('joint state: %s' % str(req.ik_request.ik_seed_state.joint_state.position))
ik_result = get_ik_srv(req)
if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
rospy.loginfo('found IK solution for given grasping pose')
rospy.loginfo('solution joints: %s' % str(ik_result.solution.joint_state.name))
rospy.loginfo('solution angles: %s' % str(ik_result.solution.joint_state.position))
return ik_result.solution
else:
rospy.logerr('Inverse kinematics failed')
return None
def __init__(self):
rospy.init_node('ik_test')
self.ik_info = '/pr2_right_arm_kinematics/get_ik_solver_info'
self.set_planning_scene_diff_name = '/environment_server/set_planning_scene_diff'
print "waiting for ik service"
rospy.wait_for_service(self.ik_info)
self.ik_info_service = rospy.ServiceProxy(self.ik_info,
GetKinematicSolverInfo)
ik_info_req = GetKinematicSolverInfoRequest()
self.ik_info = self.ik_info_service.call(ik_info_req)
#print "IK INFO:"
#print self.ik_info
#print "waiting for planning scene service"
#rospy.wait_for_service(self.set_planning_scene_diff_name)
#self.set_planning_scene_diff = rospy.ServiceProxy(self.set_planning_scene_diff_name, SetPlanningSceneDiff)
#self.set_planning_scene_diff.call(SetPlanningSceneDiffRequest())
self.ik_service = rospy.ServiceProxy(
'/pr2_right_arm_kinematics/get_ik', GetPositionIK, True)
self.ik_req = GetPositionIKRequest()
self.ik_req.timeout = rospy.Duration(1)
self.ik_req.ik_request.ik_link_name = self.ik_info.kinematic_solver_info.link_names[
-1]
self.ik_req.ik_request.ik_seed_state.joint_state.name = self.ik_info.kinematic_solver_info.joint_names
self.ik_req.ik_request.ik_seed_state.joint_state.position = [0] * 7
self.ps = PoseStamped()
self.ps.header.stamp = rospy.Time(0)
self.ps.header.frame_id = 'torso_lift_link'
self.ps.pose.position.x = self.ps.pose.position.y = self.ps.pose.position.z = 0
self.ps.pose.orientation.x = self.ps.pose.orientation.y = self.ps.pose.orientation.z = 0
self.ps.pose.orientation.w = 1
self.results = [[
88 for i in xrange(x_steps * y_steps * z_steps)
], [PoseStamped() for i in xrange(x_steps * y_steps * z_steps)]]
print len(self.results[0])
self.count = 0
self.run_poses()
def __init__(self, controller_name, service_name):
rospy.loginfo('waiting for service: %s' % service_name)
rospy.wait_for_service(service_name)
self.proxy = rospy.ServiceProxy(service_name, GetPositionIK)
# handle joint states for seek
self.joint_names = getJointNames(controller_name)
self.joint_idx = None
self.positions = None
rospy.Subscriber('/joint_states', JointState, self.js_callback)
# set static fields of request
self.request = GetPositionIKRequest()
self.request.timeout = rospy.Duration.from_sec(5.0)
self.request.ik_request.ik_link_name = 'r_wrist_roll_link'
self.request.ik_request.pose_stamped.header.frame_id = "torso_lift_link"
self.request.ik_request.ik_seed_state.joint_state.name = self.joint_names
self.request.ik_request.ik_seed_state.joint_state.position = [0] * len(
self.joint_names)
def __init__(self, side_prefix):
self.side_prefix = side_prefix
# Set up Inversse Kinematics services
ik_info_srv_name = ('pr2_' + self._side() +
'_arm_kinematics_simple/get_ik_solver_info')
ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
rospy.loginfo('Waiting for IK info service to respond.')
rospy.wait_for_service(ik_info_srv_name)
ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
GetKinematicSolverInfo)
solver_info = ik_info_srv()
self.ik_joints = solver_info.kinematic_solver_info.joint_names
self.ik_limits = solver_info.kinematic_solver_info.limits
rospy.loginfo('Waiting for IK service to respond.')
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK,
persistent=True)
# Set up common parts of an IK request
self.ik_request = GetPositionIKRequest()
self.ik_request.timeout = rospy.Duration(4.0)
self.ik_request.ik_request.ik_link_name = solver_info.kinematic_solver_info.link_names[
0]
self.ik_request.ik_request.pose_stamped.header.frame_id = 'base_link'
self.ik_request.ik_request.ik_seed_state.joint_state.name = self.ik_joints
self.ik_request.ik_request.ik_seed_state.joint_state.position = [
0
] * len(self.ik_joints)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(motion_request_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory motion request server on arm: '
+ side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('IK ready for arm: ' + side_prefix)
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 = GetPositionIKRequest()
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 - self.offset)
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]
print "roll", e[0]
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:
print "Service did not process request: %s" % str(e)
ik_service = rospy.ServiceProxy( ik_service_name, GetPositionIK )
rospy.loginfo( 'ik service %s is up and running!'%ik_service_name )
traj_filter = TrajectoryFilter( 'trajectory_filter/filter_trajectory_with_constraints' )
neutral = [ np.pi / 3,
np.pi / 3,
0,
-3 * np.pi/4,
0,
0,
0]
# creating way points in joint space
rospy.loginfo( 'creating trajectory ...' )
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = "l_wrist_roll_link"
req.ik_request.pose_stamped.header.frame_id = "torso_lift_link"
req.ik_request.ik_seed_state.joint_state.name = joint_names
req.ik_request.ik_seed_state.joint_state.position = neutral
joint_positions = []
center = Pose( position = Point( 0.4, 0.2, 0.0 ),
orientation = Quaternion( 0.0, 0.0, 0.0, 1.0 ) )
home_pose = Pose( position = Point( 0.25, 0.55, 0.0 ),
orientation = Quaternion( 0.0, 0.0, 0.0, 1.0 ) )
req.ik_request.pose_stamped.pose = home_pose
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