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 callback(comm):
msg = PoseStamped()
msg.header.seq = 0
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = 'base'
msg.pose = comm.pose
arr = SolvePositionIKRequest()
arr.pose_stamp.append(msg)
arr.seed_mode = 0
print arr
left_ik_addr = '/ExternalTools/left/PositionKinematicsNode/IKService'
right_ik_addr = '/ExternalTools/right/PositionKinematicsNode/IKService'
if rospy.get_param('/robot/unityros/movelock') == True:
return
try:
left_ik = rospy.ServiceProxy(left_ik_addr, SolvePositionIK)
right_ik = rospy.ServiceProxy(right_ik_addr, SolvePositionIK)
if comm.limb == 'left':
ik = left_ik(arr)
else:
ik = right_ik(arr)
right_limb = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=1)
left_limb = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=1)
message = JointCommand()
message.mode = 1
message.command = ik.joints[0].position
message.names = ik.joints[0].name
rospy.loginfo(message)
right_limb.publish(message)
left_limb.publish(message)
except rospy.ServiceException, e:
print 'Service call failed: %s' % e
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
ikl = rospy.ServiceProxy(
'/ExternalTools/left/PositionKinematicsNode/IKService',
SolvePositionIK)
pubr = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=1)
publ = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=1)
cmd = JointCommand()
cmd.mode = 1
req = SolvePositionIKRequest()
req.seed_mode = 0
M0 = transformations.quaternion_matrix((0.031, 0.808, .587, 0.028))
M0[0, 3] = 0.7
M0[1, 3] = -0.137
M0[2, 3] = 0.357
Md = np.eye(4)
Mrl = np.zeros((4, 4))
Mrl[2, 3] = 0.2
Mrl[2, 0] = Mrl[0, 2] = -1
Mrl[1, 1] = 1
Mrl[3, 3] = 1
print(Mrl, matrix_msg(Mrl))
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)
# Null space goals are not supported on the baxter
## 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
),
orientation=Quaternion(
x=0.0,
y=0.71,
z=0.0,
w=-0.71,
),
),
),
}
ikreq_r = SolvePositionIKRequest()
ikreq_r.pose_stamp = [poses['right']]
seed = random_seed()
ikreq_r.seed_angles = seed
ikreq_r.seed_mode = 1
print "IKREQ_FIXED = ", ikreq_r
#print "IKREQ_R : ",ikreq_r
try:
rospy.wait_for_service(ns_r, 5.0)
resp_r = iksvc_r(ikreq_r)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return
if (resp_r.isValid[0]):
print("RIGHT_SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
limb_joints_r = dict(
zip(resp_r.joints[0].name, resp_r.joints[0].position))