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 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, 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 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_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(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_request(srv, 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:
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
rospy.wait_for_service(ns, 5.0)
resp = srv(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
def Inverse_Kinematics(self, coordinates, orientation):
# Define new node
# rospy.init_node("Sawyer_ik_client")
# Create an object to interface with the arm
Robot_limb = intera_interface.Limb('right')
# Find the Equivalent gamma (yaw), beta (pitch) and alpha (roll)
quaternion = tf.transformations.quaternion_from_euler(
math.radians(orientation[0]), math.radians(orientation[1]),
math.radians(orientation[2]))
# Get the current state of the robot
angles = Robot_limb.joint_angles()
# Initialize the IK service
limb = "right"
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# Define the pose
poses = {
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=coordinates[0],
y=coordinates[1],
z=coordinates[2],
),
orientation=Quaternion(
x=quaternion[0],
y=quaternion[1],
z=quaternion[2],
w=quaternion[3],
),
),
),
}
# 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')
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
joint_seed = [None] * 7
joint_seed[0] = angles['right_j0']
joint_seed[1] = angles['right_j1']
joint_seed[2] = angles['right_j2']
joint_seed[3] = angles['right_j3']
joint_seed[4] = angles['right_j4']
joint_seed[5] = angles['right_j5']
joint_seed[6] = angles['right_j6']
seed = JointState()
seed.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
seed.position = joint_seed
ikreq.seed_angles.append(seed)
# Optimize the null space in terms of joint configuration
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_j2']
goal.position = [0]
ikreq.nullspace_goal.append(goal)
# Define the gain of the gradient descent essentially
ikreq.nullspace_gain.append(0.4)
# Get the response from the server
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
def ik_service_client(self,
goal=None,
limb="right",
use_advanced_options=False):
# Setup for the service request
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
if goal is None:
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
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,
),
),
),
}
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses[limb])
else:
ikreq.pose_stamp.append(goal)
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
if use_advanced_options:
# 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.")
# Call the IK Service
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 convertToJointStates(self, target):
"""
Using IK solver to convert the target position to Joint Space from Cartesian Space
@param target: Pose() msg to be converted
@return type: returns a list of joint values
"""
ikreq = SolvePositionIKRequest()
p = PoseStamped()
p.header = Header(stamp=rospy.Time.now(), frame_id='base')
p.pose = target
print "========= target 0 ========="
print target
self.target.position.x = p.pose.position.x
self.target.position.y = p.pose.position.y
self.target.position.z = p.pose.position.z
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(p)
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = self.right_arm
# use random numbers to get different solutions
# j1 = random.randint(50,340)/100.0
# j2 = random.randint(40,300)/100.0
# j3 = random.randint(-180,140)/100.0
# j4 = random.randint(-150,10)/100.0
# j5 = random.randint(-180,10)/100.0
# j6 = random.randint(-200,10)/100.0
# j7 = random.randint(-100,200)/100.0
# seed.position = [j1, j2, j3, j4, j5, j6, j7]
seed.position = self.group.get_random_joint_values()
ikreq.seed_angles.append(seed)
# get the response from IK solver Service
resp = self.ik_serv.call(ikreq)
# reassgin a random value to joint seed if fail to get the valid solution
while resp.result_type[0] <= 0:
rospy.loginfo("error type: " + str(resp.result_type[0]))
seed.position = self.group.get_random_joint_values()
ikreq.seed_angles.append(seed)
resp = self.ik_serv.call(ikreq)
# message print out
rospy.loginfo("SUCCESS - Valid Joint Solution Found")
rospy.loginfo("Solution is:\n%s", resp.joints[0].position)
# rospy.logdebug("Response message: " + str(resp))
target_js = [resp.joints[0].position[0],
resp.joints[0].position[1],
resp.joints[0].position[2],
resp.joints[0].position[3],
resp.joints[0].position[4],
resp.joints[0].position[5],
resp.joints[0].position[6]]
return target_js
def ik_service_client(limb, pos, orient, use_advanced_options = False):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK) #connection to the service
ikreq = SolvePositionIKRequest() #send a request from client to service
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(
x=orient[0],
y=orient[1],
z=orient[2],
w=orient[3],
),
),
),
}
# Add desired pose for inverse kinematics
#send to position and orientation to the service
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
if (use_advanced_options):
# 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_AUTO
#seed = JointState()
#seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
# 'right_j4', 'right_j5', 'right_j6']
#seed.position = [0.0, -1.1,0.5, 0.5, 0.5,0.5, 0.8]
# 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.name = ['right_j0']#,'right_j1']
goal.position = [0.0]#, 0.0]
#goal.position = [-0.9, -0.001,-1.0]
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.5)
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 ik_service_client(limb = "right", use_advanced_options = False):
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=0.450628752997,
y=0.161615832271,
z=0.217447307078,
),
orientation=Quaternion(
x=0.704020578925,
y=0.710172716916,
z=0.00244101361829,
w=0.00194372088834,
),
),
),
}
# 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')
if (use_advanced_options):
# 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) as e:
rospy.logerr("Service call failed: %s" % (e,))
return False
# 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')
rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
(seed_str,))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(list(zip(resp.joints[0].name, resp.joints[0].position)))
rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints)
rospy.loginfo("------------------")
rospy.loginfo("Response Message:\n%s", resp)
else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
rospy.logerr("Result Error %d", resp.result_type[0])
return False
return True
def Inverse_Kinematics(self,
limb,
coordinates,
orientation,
prev_solution=None):
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')
quaternion = tf.transformations.quaternion_from_euler(
*np.deg2rad(orientation))
poses = {
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=coordinates[0],
y=coordinates[1],
z=coordinates[2],
),
orientation=Quaternion(
x=quaternion[0],
y=quaternion[1],
z=quaternion[2],
w=quaternion[3],
),
),
),
}
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 = prev_solution if prev_solution is not None else [
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))
else:
print('Failed to find solution!')
return limb_joints
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 = JointState()
# seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
# 'right_j4', 'right_j5', 'right_j6']
# seed.position = [0.42, -0.38, -1.24, 1.78, 1.16, 1.11, 2.05]
###############################
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_j0', 'right_j1', 'right_j2', 'right_j3']
goal.position = [0.409, -0.43, -1.2, 1.79]
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.5)
# else:
# rospy.loginfo("Running Simple IK Service Client example.")
done, i = False, 0
while not done and i < 100:
try:
rospy.wait_for_service(name_of_service, 5.0)
resp = iksvc(ikreq)
done = True
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("IK service call failed: %s" % (e, ))
i += 1
if not done:
raise IOError("IK SERVICE CALL FAILED")
# 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')
# rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
# (seed_str,))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(
list(zip(resp.joints[0].name, resp.joints[0].position)))
# rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints)
# rospy.loginfo("------------------")
# rospy.loginfo("Response Message:\n%s", resp)
return limb_joints
else:
rospy.loginfo("INVALID POSE - No Valid Joint Solution Found.")
raise ValueError
def convertToJointStates(self, target):
"""
Using IK solver to convert the target position to Joint Space from Cartesian Space
@param target: Pose() msg to be converted
@return type: returns a list of joint values
"""
ikreq = SolvePositionIKRequest()
p = PoseStamped()
p.header = Header(stamp=rospy.Time.now(), frame_id='base')
p.pose = target
print "========= target 0 ========="
print target
# add target position - might be useful when add bottle in the scene
self.target = p
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(p)
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = self.right_arm
# use random numbers to get different solutions
# TODO: how to define the limitation
j1 = random.randint(50,340)/100.0
j2 = random.randint(40,300)/100.0
j3 = random.randint(-180,140)/100.0
j4 = random.randint(-150,10)/100.0
j5 = random.randint(-180,10)/100.0
j6 = random.randint(-200,10)/100.0
j7 = random.randint(-100,200)/100.0
# j1 range 0.5 to 3.4
seed.position = [j1, j2, j3, j4, j5, j6, j7]
# seed.position = [j1, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
ikreq.seed_angles.append(seed)
# get the response from IK solver Service
resp = self.ik_serv.call(ikreq)
print "========= resp 0 ========="
print resp
# reassgin a random value to joint seed if fail to get the valid solution
while resp.result_type[0] <= 0:
print "error type: "
print resp.result_type[0]
j1 = random.randint(50,340)/100.0
seed.position = [j1, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
ikreq.seed_angles.append(seed)
resp = self.ik_serv.call(ikreq)
# message print out
rospy.loginfo("SUCCESS - Valid Joint Solution Found")
# rospy.logdebug("Response message: " + str(resp))
target_js = [resp.joints[0].position[0],
resp.joints[0].position[1],
resp.joints[0].position[2],
resp.joints[0].position[3],
resp.joints[0].position[4],
resp.joints[0].position[5],
resp.joints[0].position[6]]
# rospy.logdebug("Target value: " + str(target_js))
return target_js
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
def ik_service_client(limb = "right", use_advanced_options = False, position_xyz=[0,0,0], \
orientation_xyzw=[0,0,0,0], seed_position=[0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]):
"""
:param limb: "right" default
:param use_advanced_options: False default
:param position_xyz: cartesian XYZ [0 0 0] default
:param orientation_xyzw: quaternion for identifying rotation of the end
:param seed_position: where to start the IK optimization
:return:
"""
# return value class
class ret:
def __init__(self):
self.result = False
self.angle = []
# initialize an instance of return value
ret = ret()
# call IK service
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# target pose
poses = {
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=position_xyz[0],
y=position_xyz[1],
z=position_xyz[2],
),
orientation=Quaternion(
x=orientation_xyzw[0],
y=orientation_xyzw[1],
z=orientation_xyzw[2],
w=orientation_xyzw[3],
),
),
),
}
# 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')
if (use_advanced_options):
# 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 = seed_position
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 = [1, -1, 2]
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 ret
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 flex_ik_service_client(i_Limb="right",
i_TargetPose=[0.4950628, -0.0002616, 0.3974473],
i_TargetOrientation=[
0.704020578925, 0.710172716916,
0.00244101361829, 0.00194372088834
],
i_UseAdvanced=False):
"""
i_Limb
default: "right"
-- The robots limb (For Sawyer there is really only one option which is 'right')
i_TargetPose
default: [ x= 0.4950628752997,
y= -0.000261615832271,
z= 0.397447307078] Curren path would probably collide with the table
-- The cartesian position of the outer most joint [x, y, z] (If the gripper is attatched the target z value will be changed by -0.04804363884)
i_TargetOrientation
default: [ x=0.704020578925,
y=0.710172716916,
z=0.00244101361829,
w=0.00194372088834]
-- The target orientation [x, y, z, w]
i_UseAdvanced
default: true
-- Will run some optimization after the IK result has been calculated
"""
# 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)
# 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=Pose(
position=Point(
x=i_TargetPose[0],
y=i_TargetPose[1],
z=i_TargetPose[2],
),
# A Quaternion descibes the rotation of an object in 3D, we need 4D numbers to represent this (i^2 = j^2 = k^2 = ijk = -1)
# if w = 1, then there is no rotation around the x/y/z (roll/pitch/yaw) in radians
# Apparently, w is the angle (probably in rads) by which we rotate around an axis (TODO: find out which axis)
# x,y,z DONT REPRESENT THE AXIS. Basically all 4 numbers do the same thing, values represent a point on the unit circle in 2D
# TODO: Watch Humane Rigging 03 - 3D Bouncy Ball 05 - Quaternion Rotation
orientation=Quaternion(
x=i_TargetOrientation[0],
y=i_TargetOrientation[1],
z=i_TargetOrientation[2],
w=i_TargetOrientation[3],
),
),
),
}
##################################################################################################################################################################################
#
# 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")
# TODO Look into what seed actually does
# 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
# TODO Figure out why these values are used
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)
# TODO Figure out what this means, and why the null space gain must be [0.0, 1.0] or by default 0.4 if its empty
ik_ServiceReq.nullspace_gain.append(0.4)
else:
rospy.loginfo("Using Simple IK Solver")
##################################################################################################################################################################################
#
# 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=5.0
) # 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
