def ik_service_client(self):
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(self.pose)
ikreq.tip_names.append('right_hand')
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
seed.position = [
self.curr_position["right_j0"], self.curr_position["right_j1"],
self.curr_position["right_j2"], self.curr_position["right_j3"],
self.curr_position["right_j4"], self.curr_position["right_j5"],
self.curr_position["right_j6"]
]
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 False
def get_joint_angles(pose,
seed_cmd=None,
use_advanced_options=False,
current=True):
limb = "right"
name_of_service = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(name_of_service, SolvePositionIK)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.tip_names.append('right_hand')
seed_joints = None
if use_advanced_options:
if current:
ikreq.seed_mode = ikreq.SEED_CURRENT
else:
ikreq.seed_mode = ikreq.SEED_AUTO
seed = joint_state_from_cmd(seed_cmd)
ikreq.seed_angles.append(seed)
try:
rospy.wait_for_service(name_of_service, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return False
def get_joint_angles(pose, limb="right", seed_cmd=None, use_advanced_options=False, verbosity=0):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
# Add desired pose for inverse kinematics
stamped_pose = stamp_pose(pose)
ikreq.pose_stamp.append(stamped_pose)
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
# rospy.loginfo("Running IK Service Client.")
# seed_joints = None
if use_advanced_options:
# Optional Advanced IK parameters
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
# if not seed_joints:
# seed = JointState()
# seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
# 'right_j4', 'right_j5', 'right_j6']
# seed.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# else:
seed = joint_state_from_cmd(seed_cmd)
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 False
def GetEndJoints(self):
if not self.currentJointPositions:
rospy.loginfo("Cant get current joint poses")
return {}
limb = self.endPoseName
#limb = "right"
ns = "ExternalTools/" + limb + "/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=self.endPosePosition.x,
y=self.endPosePosition.y,
z=self.endPosePosition.z,
),
orientation=Quaternion(
x=self.endOrientPosition.x,
y=self.endOrientPosition.y,
z=self.endOrientPosition.z,
w=self.endOrientPosition.w,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
ikreq.tip_names.append('right_hand')
rospy.loginfo("Running Advanced IK Service Client example.")
ikreq.seed_mode = ikreq.SEED_CURRENT
seed = JointState()
seed.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
seed.position = self.currentJointPositions[0:6]
ikreq.seed_angles.append(seed)
ikreq.use_nullspace_goal.append(True)
goal = JointState()
goal.name = ['right_j1', 'right_j2', 'right_j3']
goal.position = [0.1, 0.1, 0.1]
ikreq.nullspace_goal.append(goal)
ikreq.nullspace_gain.append(0.1)
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(limb, coordinates, orientation):
angles = limb.joint_angles()
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=coordinates.x,
y=coordinates.y,
z=coordinates.z,
),
orientation=Quaternion(
x=orientation.x,
y=orientation.y,
z=orientation.z,
w=orientation.w, ),
), ), }
ikreq.pose_stamp.append(poses['right'])
ikreq.tip_names.append('right_hand')
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']
seed.position = [angles[a] for a in seed.name]
ikreq.seed_angles.append(seed)
# Optimize the null space in terms of joint configuration
ikreq.use_nullspace_goal.append(True)
goal = JointState()
goal.name = ['right_j2']
goal.position = [0]
ikreq.nullspace_goal.append(goal)
ikreq.nullspace_gain.append(0.4)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Service call failed: %s" % (e,))
limb_joints = angles
# Check if result valid, and type of seed ultimately used to get solution
if (resp.result_type[0] > 0):
seed_str = {ikreq.SEED_USER: '******',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp.result_type[0], 'None')
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
def get_ik(pose, q_initial, verbose=True):
"""
Get the inverse kinematics of the robot for a given pose
Arguments:
pose - desired pose in the format (x,y,z,ew,ex,ey,ez), containing
the position and orientation (quaternion)
q_initial - list of initial joint configuration used as initial point
when computing the inverse kinematics
Returns:
result - structure containing the joint angles from inverse kinematics and an
indication of whether the values are valid
"""
pose_msg = pose_to_dict_message(pose)
limb = "right"
# Structure that the function returns
result = JointResult()
# Service name
serv_name = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
# Client for the inverse kinematics server
ik_client = rospy.ServiceProxy(serv_name, SolvePositionIK)
# Message for the request
ik_request = SolvePositionIKRequest()
# Add desired pose for inverse kinematics
ik_request.pose_stamp.append(pose_msg[limb])
# Request inverse kinematics from base to "right_hand" link
ik_request.tip_names.append('right_hand')
# Start the IK optimization from this joint configuration (seed)
ik_request.seed_mode = ik_request.SEED_USER
seed = JointState()
seed.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5',
'right_j6'
]
seed.position = [
q_initial['right_j0'], q_initial['right_j1'], q_initial['right_j2'],
q_initial['right_j3'], q_initial['right_j4'], q_initial['right_j5'],
q_initial['right_j6']
]
ik_request.seed_angles.append(seed)
try:
# Block until the service is available
rospy.wait_for_service(serv_name, 3.0)
# Service request
ik_response = ik_client(ik_request)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return result
def ik_service_client(self, pose1, rospy):
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose1.header = hdr
poses = {'right': pose1}
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses["right"])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
if (True):
# Optional Advanced IK parameters
#rospy.loginfo("Running Advanced IK Service Client example.")
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
ikreq.seed_angles.append(seed)
# Once the primary IK task is solved, the solver will then try to bias the
# the joint angles toward the goal joint configuration. The null space is
# the extra degrees of freedom the joints can move without affecting the
# primary IK task.
ikreq.use_nullspace_goal.append(True)
# The nullspace goal can either be the full set or subset of joint angles
goal = JointState()
goal.name = ['right_j1', 'right_j2', 'right_j3']
goal.position = [0.1, -0.3, 0.5]
ikreq.nullspace_goal.append(goal)
# The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
# If empty, the default gain of 0.4 will be used
ikreq.nullspace_gain.append(0.4)
else:
rospy.loginfo("Running Simple IK Service Client example.")
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 False
def get_joint_angles(pose, seed_cmd = None, use_advanced_options = False):
limb = "right"
name_of_service = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(name_of_service, SolvePositionIK)
ikreq = SolvePositionIKRequest()
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(pose)
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
seed_joints = None
if use_advanced_options:
# Optional Advanced IK parameters
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
# if not seed_joints:
# seed = JointState()
# seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
# 'right_j4', 'right_j5', 'right_j6']
# seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
# else:
seed = joint_state_from_cmd(seed_cmd)
ikreq.seed_angles.append(seed)
# Once the primary IK task is solved, the solver will then try to bias the
# the joint angles toward the goal joint configuration. The null space is
# the extra degrees of freedom the joints can move without affecting the
# primary IK task.
# ikreq.use_nullspace_goal.append(True)
# The nullspace goal can either be the full set or subset of joint angles
# goal = JointState()
# goal.name = ['right_j1', 'right_j2', 'right_j3']
# goal.position = [0.1, -0.3, 0.5]
# ikreq.nullspace_goal.append(goal)
# # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
# # If empty, the default gain of 0.4 will be used
# ikreq.nullspace_gain.append(0.4)
# else:
# rospy.loginfo("Running Simple IK Service Client example.")
try:
rospy.wait_for_service(name_of_service, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
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))
ikreq.tip_names.append('right_hand')
# NOT WORKING
'''
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
seed.position = [-2.7, 0.12, -1.4, -1, 1.3, 1.6, 4.6]
ikreq.seed_angles.append(seed)
# Once the primary IK task is solved, the solver will then try to bias the
# the joint angles toward the goal joint configuration. The null space is
# the extra degrees of freedom the joints can move without affecting the
# primary IK task.
ikreq.use_nullspace_goal.append(True)
# The nullspace goal can either be the full set or subset of joint angles
goal = JointState()
goal.name = ['right_j0', 'right_j1']
goal.position = [-2.7, 0.12]
ikreq.nullspace_goal.append(goal)
ikreq.nullspace_gain.append(0.4)
'''
try:
rospy.wait_for_service(self._iksvc_name, 5)
resp = self._iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return False
def ik_service_client(limb = "right", tip_name = "right_gripper_tip"):
limb_mv = intera_interface.Limb(limb)
#gripper = intera_interface.Gripper()
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# Add desired pose for inverse kinematics
current_pose = limb_mv.endpoint_pose()
#print current_pose
movement = [0.460, -0.563,0.35]
orientation = [0.0,1,0.0,0.0]
#gripper.close()
rospy.sleep(1.0)
[dx,dy,dz] = movement
[ox,oy,oz,ow] = orientation
dy = constrain(dy,-0.7596394482267009,0.7596394482267009)
dz = constrain(dz, 0.02, 1)
# up position [0.45,-0.453,0.6] 0.11 for pick up location
#poses = {'right': PoseStamped(header=hdr,pose=Pose(position=Point(x=0.450628752997,y=0.161615832271,z=0.217447307078,),
#orientation=Quaternion(x=0.704020578925,y=0.710172716916,z=0.00244101361829,w=0.00194372088834,),),),} neutral pose
#x= 0.5,y = 0.5, z= 0.5,w= 0.5 for enpoint facing forward (orientation)
#table side parametres x=0.6529605227057904, y= +-0.7596394482267009, z=0.10958623747123714)
#table straight parametres x=0.99, y=0.0, z=0.1)
poses = {
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x= dx,
y= dy,
z= dz,
),
orientation=Quaternion(
x= ox,
y= oy,
z= oz,
w= ow,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_gripper_tip') # right_hand, right_wrist, right_hand_camera
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 False
def service_request_velocity(iksvc, vel_vec, side):
"""Move in the requested direction at a constant velocity vector"""
#INCOMPLETE
ns = "ExternalTools/" + side + "/PositionKinematicsNode/IKService"
ikreq = SolvePositionIKRequest()
limb = baxter_interface.Limb(side)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
def move_gripper_to(self, x, y, z, orientation = [0,-1,0,0], timeout = 15, confirmation = True):
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
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(
x=orientation[0],
y=orientation[1],
z=orientation[2],
w=orientation[3],
),
),
)
ikreq.pose_stamp.append(pose)
ikreq.tip_names.append('right_hand')
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
print(repr(e))
#rospy.logerr("Service call failed: %s" % (e,))
return 1
def call(self, pose, tip_name="/right_gripper_tip"):
"""
Call 'IKService' service
Parameters
----------
pose : geometry_msgs.PosedStamped
The pose on which to perform Inverse Kinematics.
tip_name : str
The ending frame name in the transform tree from which to compute Inverse Kinematics.
Returns
-------
response : InverseKinematicsResponse
The InverseKinematicsResponse response built from the IKService service response.
"""
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = pose
poses.hdr = hdr
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses[self.limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append(tip_name)
try:
resp = self.service(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
def ik_service_client(position_in,orientation_in):
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=position_in[0],
y=position_in[1],
z=position_in[2],
),
orientation=Quaternion(
x=orientation_in[0],
y=orientation_in[1],
z=orientation_in[2],
w=orientation_in[3],
),
),
),
}
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses["right"])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
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 False
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 ik_service_client(limb = "right", use_advanced_options = False):
limb_mv = intera_interface.Limb('right')
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# Add desired pose for inverse kinematics
camera_pose = limb_mv.tip_state("right_hand_camera") #current right_hand_camera pose
print camera_pose
# first position [0.45,-0.45,0.30]
# second position [0.45,-0.55,0.30]
# third position [0.362,-0.45,0.30]
# fourth position [0.362,-0.55,0.30]
movement = [0.4456, -0.563, 0.335,]
[dx,dy,dz] = movement
dy = constrain(dy,-0.7596394482267009,0.7596394482267009)
dz = constrain(dz, 0.1, 1)
#x= 0.7,y = 0.7, z= 0.0,w= 0.0 for camera facing down (orientation, facing forward)
#x= 0.0,y = 1, z= 0.0,w= 0.0 for camera facing down (orientation, facing side)
poses = {
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x= dx,
y= dy,
z= dz,
),
orientation=Quaternion(
x= 0,
y= 1,
z= 0.0,
w= 0.0,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand_camera') # right_hand, right_wrist, right_hand_camera
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 False
def get_joint_angles(self, pose, seed_cmd):
"""
Convert the Pose to joint angles.
"""
ns = "ExternalTools/" + self.limb_name +\
"/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
stamped_pose = self.stamp_pose(pose)
ikreq.pose_stamp.append(stamped_pose)
ikreq.tip_names.append('right_hand')
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = seed_cmd.keys()
seed.position = seed_cmd.values()
ikreq.seed_angles.append(seed)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
return False
def ik_service_client(self, limb = "right"):
# def ik_service_client(limb = "right"):
# ns = "Externaltools/right/PositionKinematicsNode/IKService"
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
print("Performing Inverse Kinematics")
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {'right': PoseStamped(
header = hdr,
pose = Pose(
position = Point(
x = 0.650628752997,
y = 0,
z = 0.417447307078,
),
orientation = Quaternion(
x = 0.704020578925,
y = 0.710172716916,
z = 0.00244101361829,
w = 0.00194372088834,
),
),
),
}
print("poses checkpoint")
print(poses)
ikreq.pose_stamp.append(poses[limb])
ikreq.tip_names.append('right_hand')
print("ikreq checkpoint")
print("using simple IK Service Client from ik_service_client example")
try:
print("trying resp")
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
print("resp successful")
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
print("resp didnt work")
return False
print("resp checkpoint")
def ik_service_client(limb, use_advanced_options, p_x, p_y, p_z, q_x, q_y, q_z, q_w, workspace=True):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# camera_center_human_right = [0.72934375, -0.0131015625, -2.2375146484375, 0.507642578125, 2.1652548828125, 1.8803798828125, -1.083087890625]
# camera_center_human_left = [0.7083759765625, -0.0957626953125, -1.681681640625, 0.980623046875, 1.55379296875, 1.706056640625, 1.4645380859375]
# inter_position_for_demo = [0.2603701171875, -0.628283203125, -1.1051025390625, 1.3576220703125, 0.83878515625, 1.326048828125, 0.892400390625]
# if workspace:
# return_joint = camera_center_human_right
# else:
# return_joint = camera_center_human_left
# if demo:
# return_joint = inter_position_for_demo
# print type(limb)
return_joint = intera_interface.Limb(limb).joint_ordered_angles()
poses = {
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=p_x,
y=p_y,
z=p_z,
),
orientation=Quaternion(
x=q_x,
y=q_y,
z=q_z,
w=q_w,
),
),
),
}
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
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 return_joint
def service_request_pose(iksvc, raw_pose, side, blocking=True):
ns = "ExternalTools/" + side + "/PositionKinematicsNode/IKService"
ikreq = SolvePositionIKRequest()
limb = baxter_interface.Limb(side)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose_stamped = PoseStamped(header=hdr, pose=raw_pose)
ikreq.pose_stamp.append(pose_stamped)
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_service_client(poses, limb = "right", use_advanced_options = False):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
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_solver(self, poses, limb="right"):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
print("Performing Inverse Kinematics")
ikreq.pose_stamp.append(poses[limb])
ikreq.tip_names.append('right_hand')
try:
print("Calling IK Service")
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return False, resp
def waypointToJoint(self, pose):
iksvc = rospy.ServiceProxy(
'ExternalTools/right/PositionKinematicsNode/IKService',
SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
ikreq.tip_names.append('right_hand')
resp = iksvc(ikreq)
if (resp.result_type[0] > 0):
limb_joints = dict(
zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
return False
def ik_service_client(limb, use_advanced_options, p_x, p_y, p_z, q_x, q_y, q_z,
q_w):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# return_joint sends robot to pre-grasp arm configuration
return_joint = [
-1.500208984375, -1.0860322265625, -0.177197265625, 1.3819580078125,
0.0950634765625, 1.3055205078125, 1.6654560546875
]
poses = {
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=p_x, # pre-grasp: 0.5609462822565073
y=p_y, # -0.3446309287328617
z=p_z,
# 0.45777571205785983[-1.500208984375, -1.0860322265625, -0.177197265625,
# 1.3819580078125, 0.0950634765625, 1.3055205078125, 1.6654560546875]
),
orientation=Quaternion(
x=q_x, # 0.20778492941475438
y=q_y, # 0.9778261053583365
z=q_z, # -0.010705090881921715
w=q_w, # -0.023810330049445317
),
),
),
}
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
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 return_joint
def IK(object_location):
ik_service_name = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ik_service_name, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# create desired position in x,y,z (in meters)
desired_pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=object_location[0], # forward/backward direction (default 0.7)
y=object_location[1], # left/right direction
z=sawyer_z_plane, # up/down direction
),
orientation=Quaternion(
x=0.704020578925,
y=0.710172716916,
z=0.00244101361829,
w=0.00194372088834,
)
)
)
# request desired position to ik
ikreq.pose_stamp.append(desired_pose)
# assign gripper hand
ikreq.tip_names.append('right_gripper_tip')
# apply IK request as response
resp = iksvc(ikreq)
# print resp
# test target position
target_pose = dict(zip(resp.joints[0].name, resp.joints[0].position))
# move arm to target position
# dir(assignment) to see possible joint commands
limb.move_to_joint_positions(target_pose)
if limb.move_to_joint_positions(target_pose) == limb.joint_angles():
gripper.close()
def ik(self, pose, speed):
iksvc = rospy.ServiceProxy(
'ExternalTools/right/PositionKinematicsNode/IKService',
SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
ikreq.tip_names.append('right_hand')
resp = iksvc(ikreq)
# execute the joint command if the inverse kinematics is successful
if (resp.result_type[0] > 0):
limb_joints = dict(
zip(resp.joints[0].name, resp.joints[0].position))
rospy.loginfo(">>>>>>>>>> moving the arm >>>>>>>>>>")
self.basicPositionMove(limb_joints, speed)
else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
return False
def move_to(limb, position, orientation, speed):
rospy.loginfo("Starting move...")
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
poses = new_pose(position, orientation)
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
resp = iksvc(ikreq)
# reformat the solution arrays into a dictionary
joint_solution = dict(zip(resp.joints[0].name, resp.joints[0].position))
rospy.loginfo(joint_solution)
# set arm joint positions to solution
arm = intera_interface.Limb(limb)
# set speed of move
arm.set_joint_position_speed(speed)
arm.move_to_joint_positions(joint_solution, timeout=5, threshold=0.01)
rospy.loginfo('Move complete')
def flex_ik_service_client(self, i_Limb="right", i_Pose=None, i_UseAdvanced=False):
if self.is_adding_new_item == True: # Can't move if user is in close proximity manually moving the arm
return False
if i_Pose == None:
rospy.logerr("Error: flex_ik_service_client received 'None' in arg 'i_Pose' for target position")
return False
# Initialize IK service
service_name = "ExternalTools/" + i_Limb + "/PositionKinematicsNode/IKService"
ik_ServiceClient = rospy.ServiceProxy(service_name, SolvePositionIK) # Creates a handle to some service that we want to invoke methods on, in this case the SolvePositionIK
ik_ServiceReq = SolvePositionIKRequest() # Create a request message that will give us the inverse kinematics for some set of joints (Note that this is done at compile time)
msg_header = Header(stamp=rospy.Time.now(), frame_id='base') # Generates a Header for the message (Think of HTTP)
# Hover over the object
hover_pose = copy.deepcopy(i_Pose)
hover_pose.position.z = hover_pose.position.z + self.hover_dist # hover over the object
# Set the goal positions for the limb that we specified (I'm pretty sure that it's for the last joint right_j6)
# Set the header that we time stamped
# Add Pose object that contains a Point and Quaternion
goal_positions = {
'right': PoseStamped(
header=msg_header,
pose=hover_pose
),
}
##############################################################################################
#
# SOLVER IS SETUP TO FIND PATH USING SIMPLE INVERSE KINEMATICS
#
##############################################################################################
# Add the goal positions for the inverse kinematics
ik_ServiceReq.pose_stamp.append(goal_positions[i_Limb])
# Request the inverse kinematics from the base to the "right_hand" link
ik_ServiceReq.tip_names.append('right_hand')
##############################################################################################
#
# SETUP THE SOLVER TO FIND A PATH USING ADVANCED INVERSE KINEMATICS
#
##############################################################################################
if (i_UseAdvanced):
rospy.loginfo("Using Advanced IK Service")
# Define the joint seed. If the solver encounters the specified value for a joint, the it will attempt to do some optimisation
ik_ServiceReq.seed_mode = ik_ServiceReq.SEED_USER
seed = JointState() # JointState describes the state of each joint (possition, velocity, effort)
seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] # The name of the various joints
seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4] # The joint angle at which the solver tries to do optimisation for the respective joints
# Pass the seeded angles to the Solver via the SolvePositionIKRequest object
ik_ServiceReq.seed_angles.append(seed)
# NOTE Once the Primary IK Task has been solved, the IK Solver will try to bias the joint angles towards the goal joint configuration
# NOTE The null space represents the extra degrees of freedom that the joints can move through without affecting the results of the Primary IK Task
# Here we try to solve the Primary IK Task
ik_ServiceReq.use_nullspace_goal.append(True)
# NOTE The null space can either be a subset or the full set of joint angles
goal = JointState()
# This is the subset of joints that make up the null space "Joints A, B, C ... can try to move, but can't affect the result"
goal.name = ['right_j1', 'right_j2', 'right_j3']
goal.position = [0.1, -0.3, 0.5] # If these possitions are encountered, try to do some optimisation
ik_ServiceReq.nullspace_goal.append(goal)
ik_ServiceReq.nullspace_gain.append(0.4)
##############################################################################################
#
# PASS THE INVERSE KINEMATICS REQUEST TO THE SOLVER TO GET AN ACTUAL PATH FOR THE ROBOT
#
##############################################################################################
# rospy.loginfo("Simple IKService Solver Running...")
try:
rospy.wait_for_service(service_name, timeout=self.arm_timeout) # Waits for the service (5 seconds), creates the service if it doesn't already exist
response = ik_ServiceClient(ik_ServiceReq) # Get the response from the client, contains the joint positions
except (rospy.ServiceException, rospy.ROSException), ex:
rospy.logerr("Service Call Failed: %s" % (ex,))
return False
def IK(object_loc):
alignment = 0
for x in range(0,2):
object_x, object_y = pix_xy_trans(object_loc)
ik_service_name = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ik_service_name, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
if (alignment == 0):
# create desired position in x,y,z (in meters)
desired_pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=object_x, # forward/backward direction (default 0.7)
y=object_y, # left/right direction
z=sawyer_z_hover_plane, # up/down direction
),
orientation=Quaternion(
x=0.704020578925,
y=0.710172716916,
z=0.00244101361829,
w=0.00194372088834,
)
)
)
else:
# create desired position in x,y,z (in meters)
time.sleep(1)
desired_pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=object_x, # forward/backward direction (default 0.7)
y=object_y, # left/right direction
z=sawyer_z_grab_plane, # up/down direction
),
orientation=Quaternion(
x=0.704020578925,
y=0.710172716916,
z=0.00244101361829,
w=0.00194372088834,
)
)
)
# request desired position to ik
ikreq.pose_stamp.append(desired_pose)
# assign gripper hand
ikreq.tip_names.append('right_gripper_tip')
# apply IK request as response
resp = iksvc(ikreq)
# print resp
# test target position
target_pose = dict(zip(resp.joints[0].name, resp.joints[0].position))
# move arm to target position
# dir(assignment) to see possible joint commands
limb.move_to_joint_positions(target_pose)
alignment+=1
# include 'from collections import Counter' in header
# location_for_close = dict(set(limb.joint_angles().items()) - set(target_pose.items()))
# if (location_for_close[0] <= .1) and (location_for_close[1] <= .1) and (location_for_close[2] <= .1) and (location_for_close[3] <= .1):
# no need for above, gripper will close after moving to target position
time.sleep(1)
gripper.close()
def ik_request(self, pose,
end_point='right_hand', joint_seed=None,
nullspace_goal=None, nullspace_gain=0.4,
allow_collision=False):
"""
Inverse Kinematics request sent to IK Service
@type pose: geometry_msgs.Pose
@param pose: Cartesian pose of the end point
@type end_point: string
@param end_point: name of the end point (should be in URDF)
@type joint_seed: dict({str:float})
@param joint_seed: the joint angles for the initial IK guess (optional)
@type nullspace_goal: dict({str:float})
@param nullspace_goal: desired joints, or subset of joints, to bias the solver (optional)
@type nullspace_gain: double
@param nullspace_gain: gain used to bias toward the nullspace goal [0.0, 1.0] (optional)
@type allow_collision: bool
@param allow_collision: does not check if Ik solution is in collision
@rtype: dict({str:float})
@return: valid joint positions if exists. False if no solution is found.
"""
if not isinstance(pose, Pose):
rospy.logerr('pose is not of type geometry_msgs.msgs.Pose')
return False
# Add desired pose for inverse kinematics
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
ikreq.tip_names.append(end_point)
# The joint seed is where the IK position solver starts its optimization
if joint_seed is not None:
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = list(joint_seed.keys())
seed.position = list(joint_seed.values())
ikreq.seed_angles.append(seed)
# Once the primary IK task is solved, the solver will then try to bias the
# the joint angles toward the goal joint configuration. The null space is
# the extra degrees of freedom the joints can move without affecting the
# primary IK task.
if nullspace_goal is not None:
ikreq.use_nullspace_goal.append(True)
# The nullspace goal can either be the full set or subset of joint angles
goal = JointState()
goal.names = list(nullspace_goal.keys())
goal.position = list(nullspace_goal.values())
ikreq.nullspace_goal.append(goal)
# The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
# If empty, the default gain of 0.4 will be used
ikreq.nullspace_gain.append(nullspace_gain)
try:
resp = self._iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("IK Service call failed: %s" % (e,))
return False
limb_joints = {}
# Check if result valid, and type of seed ultimately used to get solution
if (resp.result_type[0] > 0
or (allow_collision and resp.result_type[0] == resp.IK_IN_COLLISION)):
# Format solution into Limb API-compatible dictionary
limb_joints = dict(list(zip(resp.joints[0].name, resp.joints[0].position)))
else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
return False
return limb_joints
