def solveIK(self, des_pose):
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose = Pose()
quat = Quaternion()
if len(des_pose) == 6:
#Note that this may be wrong!
quatArray = quaternion_from_euler(des_pose[3],
des_pose[4],
des_pose[5],
axes='sxyz')
quat.x = quatArray[0]
quat.y = quatArray[1]
quat.z = quatArray[2]
quat.w = quatArray[3]
else:
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]
# print pose.position
ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)]
try:
rospy.wait_for_service(self.ns, 5.0)
resp = self.iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("IK Service Exception thrown")
return None
def main():
rospy.init_node("scan_initial")
#Create a publisher
right = baxter_interface.Limb('right')
# Call service
ns_r = "ExternalTools/" + 'right' + "/PositionKinematicsNode/IKService"
iksvc_r = rospy.ServiceProxy(ns_r, SolvePositionIK)
ikreq_r = SolvePositionIKRequest()
# Customize message
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
#z-axis and orientation must be kept in this form for baxter to reach outward
poses = {
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.85,
y=-0.39,
z=-0.00,
),
orientation=Quaternion(
x=0.00,
y=0.71,
z=0.0,
w=0.71,
),
),
),
}
ikreq_r.pose_stamp = [poses['right']]
print 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
def move_pos(goal, limb, timeout=3):
# Call Baxter's inbuilt service
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
# Request for same service
ikreq = SolvePositionIKRequest()
# Fill in all message parts
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# Define required x, y, z parts
pose = Pose()
# Define orientation of gripper.
quat = Quaternion()
quat.x = goal[3]
quat.y = goal[4]
quat.z = goal[5]
quat.w = goal[6]
# Fill the position vectors from goal configuration
pose.orientation = quat
pose.position.x = goal[0]
pose.position.y = goal[1]
pose.position.z = goal[2]
ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)]
# Using Random seeds till a "VALID" IK solution is found
# FOR SEED METHOD:
# cite1: IK_Service_Client in package: Baxter_Examples
# cite2: test_moveit.py from NXR repo linked in Course notes
try:
rospy.wait_for_service(ns, 3.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.loginfo("Service exception")
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 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")
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")
if resp.isValid[0]:
des_joints = [0]*7
for i in range(7):
des_joints[i] = resp.joints[0].position[i]
# print right_arm_group.get_current_pose()
# print des_joints
right_arm_group.set_joint_value_target(des_joints)
def move_callback(data):
#set object size force parameters
side = data.side
# print side
point = data.point
# print point
quaternion = data.quaternion
# print quaternion
speed = data.speed
# print speed
npoints = data.npoints
ns = "ExternalTools/" + side + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
arm = baxter_interface.Limb(side)
arm.set_joint_position_speed(0.5 * speed)
x, y, z = point.x, point.y, point.z
if side == 'right':
cur_x = position_now_R.x
cur_y = position_now_R.y
cur_z = position_now_R.z
elif side == 'left':
cur_x = position_now_L.x
cur_y = position_now_L.y
cur_z = position_now_L.z
else:
raise ValueError(
"INVALID SIDE PASSED TO MOVE.PY, OPTIONS ARE 'left' OR 'right'")
dx = x - cur_x
dy = y - cur_y
dz = z - cur_z
print 'dx, dy, dz', dx, dy, dz
if npoints == 0:
dist = np.sqrt(dx**2 + dy**2 + dz**2)
npoints = int(dist / 0.01)
# print npoints
xs = np.linspace(x - dx, x, npoints, endpoint=True)
ys = np.linspace(y - dy, y, npoints, endpoint=True)
zs = np.linspace(z - dz, z, npoints, endpoint=True)
for i in np.arange(npoints):
pose = {
side:
PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
# x = x[i],
# y = y[i],
# z = z[i],
x=xs[i],
y=ys[i],
z=zs[i],
),
# e-e must be maintained in this orientation
orientation=Quaternion(
x=quaternion.x,
y=quaternion.y,
z=quaternion.z,
w=quaternion.w,
),
),
),
}
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp = [pose[side]]
count = 0
while count < 3:
try:
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return
if (resp.isValid[0]):
limb_joints = dict(
zip(resp.joints[0].name, resp.joints[0].position))
# arm.set_joint_positions(limb_joints)
if i == npoints - 1 and npoints > 10 or npoints == 1: # SWAG
arm.move_to_joint_positions(limb_joints)
else:
arm.set_joint_positions(limb_joints)
rospy.sleep(0.025 / speed)
break
else:
print("INVALID POSE - No Valid Joint Solution Found.")
count += 1
while not rospy.is_shutdown():
tr.header.stamp = rospy.Time.now()
br.sendTransform(tr)
# update pose
Md[0, 3] = 0.05 * np.cos(t / 2.)
Md[1, 3] = 0.1 * np.sin(t / 2)
Md[2, 3] = 0.2 * np.cos(t / 2)
Md[:3, :3] = transformations.euler_matrix(.1 * np.cos(t),
.1 * np.sin(t / 2),
.05 * np.cos(t + 4))[:3, :3]
M = np.dot(M0, Md)
req.pose_stamp = [matrix_msg(M)]
res = ikr(req)
cmd.names = res.joints[0].name
cmd.command = res.joints[0].position
pubr.publish(cmd)
req.pose_stamp = [matrix_msg(np.dot(M, Mrl))]
res = ikl(req)
cmd.names = res.joints[0].name
cmd.command = res.joints[0].position
publ.publish(cmd)
t += dt
rospy.sleep(dt)
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")
if resp.isValid[0]:
des_joints = [0] * 7
for i in range(7):
des_joints[i] = resp.joints[0].position[i]
# print right_arm_group.get_current_pose()
# print des_joints
right_arm_group.set_joint_value_target(des_joints)
x=0.85,
y=0.00,
z=0.00,
),
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:")
