def _get_ik_robot(self, eef_poses, seeds=(), params=None):
ik_req = SolvePositionIKRequest()
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
ik_req.seed_mode = ik_req.SEED_USER if len(seeds) > 0 else ik_req.SEED_CURRENT
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['robot']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def move_pos(des_pose):
limb = "right"
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# quat = quaternion_from_euler(des_pose[3],des_pose[4],des_pose[5])
pose = Pose()
quat = Quaternion()
quat.x = des_pose[3]
quat.y = des_pose[4]
quat.z = des_pose[5]
quat.w = des_pose[6]
pose.orientation = quat
pose.position.x = des_pose[0]
pose.position.y = des_pose[1]
pose.position.z = des_pose[2]
ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)]
# print ikreq
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.loginfo("Service exception")
def ik_request(self, pose):
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
try:
resp = self._iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
def baxter_ik_move(self, rpy_pose):
#quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
quaternion_pose = convert_pose_to_msg(rpy_pose, "base")
node = "ExternalTools/" + self.limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message, ))
print "ERROR - baxter_ik_move - Failed to append pose"
return 1
def baxter_sdk_ik_request(self, pose):
""" Try to find a solution to the inverse kinematics of a given pose.
paramters
---------
pose: requested pose to find joint angles to reach it.
"""
header = Header(stamp=rospy.Time.now(), frame_id='base')
inverse_kinematics_request = SolvePositionIKRequest()
inverse_kinematics_request.pose_stamp.append(
PoseStamped(header=header, pose=pose))
try:
result = self.__iksvc(inverse_kinematics_request)
except (ropsy.ServiceException, rospy.ROSException), e:
rospy.logerr('service call failed: {}'.format(e))
return False
def ik_move(limb, pose):
rospy.init_node("move_" + limb)
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ps = PoseStamped(
header=hdr,
pose=pose,
)
ikreq.pose_stamp.append(ps)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
def move_to(self, pose_list, arm):
new_pose = self.make_pose_stamp(pose_list, \
Header(stamp=rospy.Time.now(), frame_id='base'))
ns = "ExternalTools/" + arm + "/PositionKinematicsNode/IKService"
print ns
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(new_pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return 0
def __init__(self):
rospy.init_node("baxter_test")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
self._rate = 5.0 #Hz
#IK
self.ns = "ExternalTools/left/PositionKinematicsNode/IKService"
self.hdr = Header(stamp = rospy.Time(0), frame_id = "base")
self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK)
self.ikreq = SolvePositionIKRequest()
self._left_arm = baxter_interface.limb.Limb("left")
self._right_arm = baxter_interface.limb.Limb("right")
self._left_joint_names = self._left_arm.joint_names()
self._right_joint_names = self._right_arm.joint_names()
self._left_angles = [self._left_arm.joint_angle(joint) for joint in self._left_joint_names]
self._right_angles = [self._right_arm.joint_angle(joint) for joint in self._right_joint_names]
def inverse_kinematics(self, pose):
"""Determine the joint angles that provide a desired pose for the
robot"s end-effector."""
quaternion_pose = conversions.list_to_pose_stamped(pose, "base")
node = "ExternalTools/" + self.name + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: {}".format(error_message, ))
sys.exit("ERROR: Failed to append pose.")
def solve(self, pose):
posestamped = geometry_msgs.msg.PoseStamped()
posestamped.pose = pose
posestamped.header.stamp = rospy.Time.now()
posestamped.header.frame_id = 'base'
req = SolvePositionIKRequest()
req.pose_stamp.append(posestamped)
rospy.wait_for_service(
'/ExternalTools/left/PositionKinematicsNode/IKService')
try:
resp = self.ik_service(req)
if resp.isValid[0] == True:
return resp
else:
rospy.logerr("Resolver without solution..........\n")
return None
except rospy.ServiceException as exc:
rospy.logerr("Error requesting service:" + str(exc))
def calculate_joints(self):
# Inverse Kinematics, could not get the task to work with this, left for later projects
header = Header(stamp=rospy.Time.now(), frame_id='base')
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(PoseStamped(header=header, pose=self.pose))
ik_namespace = 'ExternalTools/' + self.limb_name + '/PositionKinematicsNode/IKService'
try:
ik_service = rospy.ServiceProxy(ik_namespace, SolvePositionIK)
ik_response = ik_service(ik_request)
ik_validation = struct.unpack('<%dB' % len(ik_response.result_type), ik_response.result_type)[0]
if (ik_validation != ik_response.RESULT_INVALID):
return dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
else:
rospy.logerr('No Valid Pose Found!')
return False
except(rospy.ServiceException, rospy.ROSException), e:
rospy.logerr('Service call failed: %s' % (e,))
return False
def solve(self, limb, pose):
# pose = h.list_to_pose(pose_arr)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose = PoseStamped(header=hdr, pose=pose)
ikreq.pose_stamp.append(pose)
if limb is "left":
ns = self.l_ns
iksvc = self.l_iksvc
else:
ns = self.r_ns
iksvc = self.r_iksvc
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return None
def ik_test(limb):
rospy.init_node("rsdk_ik_service_client")
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
'left': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.657579481614,
y=0.851981417433,
z=0.0388352386502,
),
orientation=Quaternion(
x=-0.366894936773,
y=0.885980397775,
z=0.108155782462,
w=0.262162481772,
),
),
),
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=-0.0100696086884,
y=0.0856908559799,
z=0.621000051498,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
def __init__(self, limb, side):
# TODO: Replace limb motion with moveit motions
self.side = side
# TODO: JB Added
# self._limb = baxter_interface.Limb(limb)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self._limb = moveit_commander.MoveGroupCommander(limb)
self._limb.set_max_velocity_scaling_factor(0.4)
circle_io = baxter_interface.DigitalIO(side + '_lower_button')
dash_io = baxter_interface.DigitalIO(side + '_upper_button')
self.calibrating = False
self.object_calib = -1
self.objects = [
'neutral', 'Cup', 'Tea', 'Sugar', 'Left_Bread', 'Right_Bread',
'Lettuce', 'Meat'
]
# self.objects = ['neutral', 'placemat', 'cup', 'plate', 'fork', 'spoon', 'knife', 'bowl', 'soda', 'wineglass']
# self.objects = ['neutral', 'cup', 'plate', 'bowl']
# self.objects = ['neutral', 'bowl']
# TODO: JB Added
# self.object_pick_joint_angles = dict()
self.object_pick_poses = dict()
# TODO: JB Added
# self.object_place_joint_angles = dict()
self.object_place_poses = dict()
self._gripper = baxter_interface.Gripper(side)
self._gripper.calibrate()
self._gripper.set_holding_force(100.0)
ik_srv = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK)
self._ikreq = SolvePositionIKRequest()
circle_io.state_changed.connect(self.CirclePressed_)
# Variables to manage threads
self.work_thread = None
self.stop = False
# state variables
self.state = STATE.IDLE
def get_ik_joints_linear(initial_x, initial_y, initial_z, initial_w2, target_x,
target_y, target_z, target_w2, n_steps):
ns = "ExternalTools/left/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest(
seed_mode=SolvePositionIKRequest.SEED_CURRENT)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# current_pose = arm.endpoint_pose()
x0 = initial_x
y0 = initial_y
z0 = initial_z
# linear interpolate between current pose and target pose
for i in xrange(n_steps):
t = (i + 1) * 1. / n_steps
x = (1. - t) * x0 + t * target_x
y = (1. - t) * y0 + t * target_y
z = (1. - t) * z0 + t * target_z
pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=x,
y=y,
z=z,
),
# endeffector pointing down
orientation=Quaternion(
x=1.,
y=0.,
z=0.,
w=0.,
),
),
)
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return []
def ik():
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.8,
y=-0.32,
z=0.25,
),
orientation=Quaternion(
x=-0.007,
y=0.697,
z=-0.029,
w=0.716,
),
),
),
}
limb_joints = {
'right_s0': 0.787,
'right_s1': -0.635,
'right_w0': -0.068,
'right_w1': -1.445,
'right_w2': 0.0318,
'right_e0': -0.082,
'right_e1': 2.027,
}
ikreq.pose_stamp.append(poses['right'])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return 1
def _find_joint_position(self, pose, x_off=0.0, y_off=0.0, z_off=0.0):
'''
Finds the joint position of the arm given some pose and the
offsets from it (to avoid opening the structure all the time
outside of the function).
'''
ik_request = SolvePositionIKRequest()
the_pose = deepcopy(pose)
the_pose['position'] = Point(x=pose['position'].x + x_off,
y=pose['position'].y + y_off,
z=pose['position'].z + z_off)
approach_pose = Pose()
approach_pose.position = the_pose['position']
approach_pose.orientation = the_pose['orientation']
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose_req = PoseStamped(header=hdr, pose=approach_pose)
ik_request.pose_stamp.append(pose_req)
resp = self._iksvc(ik_request)
return dict(zip(resp.joints[0].name, resp.joints[0].position))
def callBuiltInIK(pos, rot):
# Try calling the built-in IK
req = SolvePositionIKRequest()
req_head = Header(stamp=rospy.Time.now(), frame_id='base')
req_pose = PoseStamped(header=req_head,
pose=Pose(position=pos, orientation=rot))
req.pose_stamp.append(req_pose)
# Call the services
rospy.wait_for_service(
'ExternalTools/left/PositionKinematicsNode/IKService')
try:
ik_solver = rospy.ServiceProxy(
'ExternalTools/left/PositionKinematicsNode/IKService',
SolvePositionIK)
response = ik_solver(req)
return response
except rospy.ServiceException, e:
print "Call failed %s" % e
def ik_test(pose, printing=False):
"""
From baxter example code to return a joint_angle dictionary from an input
workspace pose
"""
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return (1, limb.joint_angles()
) #returning current joint angles if not valid
def svc_move_to_AR_tag(self, data):
'''
This service function takes the cached pose information for the lid's AR marker, calls for an IK solution that
drings the designated end effector into alignment with the lid, and executes a move to the calculated joint
positions.
'''
# Establish connection to specific limb's IKSolver service
ns = '/ExternalTools/' + self.limb + '/PositionKinematicsNode/IKService'
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
# Update header information based on current time and with reference to base frame
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# Update orientation of gripper based on rotation of AR tag frame to a frame compatible with Baxter's EE state
tag_q_old = [
self.tag.orientation.x, self.tag.orientation.y,
self.tag.orientation.z, self.tag.orientation.w
]
tag_q_rot = tf.transformations.quaternion_from_euler(0, math.pi, 0)
tag_q_new = tf.transformations.quaternion_multiply(
tag_q_rot, tag_q_old)
# Locally relevant AR tag derived poses (typ. for marker, bottle, or table)
AR_pose = PoseStamped(header=hdr,
pose=Pose(position=self.tag.position,
orientation=Quaternion(
x=tag_q_new[0],
y=tag_q_new[1],
z=tag_q_new[2],
w=tag_q_new[3])))
# Set the desired pose in the service request message to pose information pulled from the object pose topic
ikreq.pose_stamp.append(AR_pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call fault: %s" % (e, ))
return (False,
"MOTION CTRL - Service call to Baxter's IK solver failed.")
def ik_solve(limb, pos, orient):
#~ rospy.init_node("rsdk_ik_service_client")
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
print "iksvc: ", iksvc
print "ikreq: ", ikreq
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
str(limb): PoseStamped(header=hdr,
pose=Pose(position=pos, orientation=orient))}
ikreq.pose_stamp.append(poses[limb])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
def get_ik(self, iksvc, x, y, z, er, ep, ey, jointAngles):
#print er, ep, ey
q = quaternion_from_euler(er, ep, ey)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose = PoseStamped(header=hdr, pose=Pose(position=Point(x=x,y=y,z=z), orientation=Quaternion(q[0], q[1], q[2], q[3])))
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
try:
if(iksvc is self.left_iksvc):
rospy.wait_for_service(BaxterAction.IKSVC_LEFT_URI, 5.0)
else:
rospy.wait_for_service(BaxterAction.IKSVC_RIGHT_URI, 5.0)
resp = iksvc(ikreq)
except(rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
def baxter_ik_solve(limb, pose, frame):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
#hdr = Header(stamp=rospy.Time.now(), frame_id='reference/right_hand_camera')
hdr = Header(stamp=rospy.Time.now(), frame_id=frame)
ps = PoseStamped(
header=hdr,
pose=pose,
)
global posedebug
target_topic.publish(ps)
ikreq.pose_stamp.append(ps)
try:
#print 'ik_solve: waiting...'
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return 1
def _get_ik(self, iksvc, pos, rot):
q = quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(q[0], q[1], q[2], q[3])),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
iksvc.wait_for_service(5.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def getIKGripper(self, setPose):
# Prepare the request for Baxter's IK
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ps = PoseStamped(
header=hdr,
pose=setPose,
)
ikreq = SolvePositionIKRequest([ps], [], 0)
# Make the service call
srvName = 'ExternalTools/' + self.side + '/PositionKinematicsNode/IKService'
srvAlias = rospy.ServiceProxy(srvName, SolvePositionIK)
rospy.wait_for_service(srvName)
resp = srvAlias(ikreq)
# Get IK response and convert to joint position dict
if (resp.isValid[0]):
return dict(zip(resp.joints[0].name, resp.joints[0].position))
else:
return None
def ObtainCartesianPos(limb, pos):
try:
srv = '/ExternalTools/' + limb.name + '/PositionKinematicsNode/IKService'
#print "Test: Service availability: %s" % srv
rospy.wait_for_service(srv, 5.0)
iksvc = rospy.ServiceProxy(srv, SolvePositionIK)
ikreq = SolvePositionIKRequest()
pose = PoseStamped(
header=std_msgs.msg.Header(
stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position = pos[0],
orientation = pos[1]
)
)
ikreq.pose_stamp.append(pose)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
def _find_approach(self, pose, offset):
ikreq = SolvePositionIKRequest()
# Add 5 cm offset in Z directio
try:
pose['position'] = Point(x=pose['position'][0],
y=pose['position'][1],
z=pose['position'][2] + offset)
except Exception:
pose['position'] = Point(x=pose['position'].x,
y=pose['position'].y,
z=pose['position'].z + offset)
approach_pose = Pose()
approach_pose.position = pose['position']
approach_pose.orientation = pose['orientation']
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose_req = PoseStamped(header=hdr, pose=approach_pose)
ikreq.pose_stamp.append(pose_req)
resp = self._iksvc(ikreq)
return dict(zip(resp.joints[0].name, resp.joints[0].position))
def ik_request(self, gripper, pose):
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq = SolvePositionIKRequest()
print("IK REQUEST ***************************")
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
try:
print("GRIPPER NAME: " + gripper.name)
if (gripper.name == "left_gripper" or gripper.name == "left"):
# print("IK request is going to be for LEFT GRIPPER")
resp = self._iksvc_left(ikreq)
elif (gripper.name == "right_gripper" or gripper.name == "right"):
# print("IK request is going to be for RIGHT GRIPPER")
resp = self._iksvc_right(ikreq)
else:
print("Unable to resolve gripper for IK_REQUEST")
exit(1)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return False
def ik_request(self, pose, seed_list=None, _joint_names=None):
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
if seed_list is not None:
for seed_angles in seed_list:
seed = JointState()
seed.name = _joint_names
seed.position = seed_angles
seed.header = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.seed_angles.append(seed)
try:
resp = self._iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return False
def Compute_IKin(limb, seed_name, seed_pos, pose):
# ROS Params initalization
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
seed = JointState()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
limb_joints = {}
ikreq.pose_stamp.append(pose[0])
seed.name = seed_name
seed.position = seed_pos
ikreq.seed_angles.append(seed)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
# except (rospy.ServiceException, rospy.ROSException), e:
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
#Check if result is valid + type of seed
resp_seeds = struct.unpack('<%dB' % len(resp.result_type),resp.result_type)
if (resp_seeds[0] != resp.RESULT_INVALID):
seed_str = {
ikreq.SEED_USER: '******',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None')
print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %(seed_str,))
#Format solution
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
#print "\nIK Joint Solution:\n", limb_joints
prev_name = resp.joints[0].name
prev_pos = resp.joints[0].position
else:
print ("INVALID POSE")
return limb_joints,prev_name,prev_pos
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def right_ik_callback(self, poseIn):
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
command = PoseStamped(header=hdr, pose=poseIn)
currentAngles = self.rightLimb.joint_angles()
ikreq.pose_stamp.append(command)
seed = JointState(name=currentAngles.keys(),
position=currentAngles.values())
ikreq.seed_angles.append(seed)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return 1
from tf.transformations import quaternion_from_euler
if __name__=='__main__':
rospy.loginfo("Starting test_moveit node")
rospy.init_node('test_moveit', log_level=rospy.INFO)
right_arm_group = moveit_commander.MoveGroupCommander("right_arm")
des_pose = [0.28, -0.62, -0.32, 0, 3.14/2, 0]
# des_pose = [0.815, -1.01, 0.321, 0.271, 0.653, -0.271, 0.653]
limb = "right"
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# quat = quaternion_from_euler(des_pose[3],des_pose[4],des_pose[5])
pose = Pose()
quat = Quaternion()
quat.x = des_pose[3]
quat.y = des_pose[4]
quat.z = des_pose[5]
quat.w = des_pose[6]
pose.orientation = quat
pose.position.x = des_pose[0]
pose.position.y = des_pose[1]
pose.position.z = des_pose[2]
ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)]
# print ikreq
try: