def pose(self):
"""
@rtype: geometry_msgs.Pose
"""
return conversions.list_to_pose_stamped(
self._robot._r.get_link_pose(self._name),
self._robot.get_planning_frame())
def ik_solver_request(input_limb, input_pose):
print "IK solver request:"
#input error checking
if len(input_pose) == 6:
quaternion_pose = conversions.list_to_pose_stamped(input_pose, "base")
elif len(input_pose) == 7:
quaternion_pose = input_pose
else:
print """Invalid Pose List:
Input Pose to function: ik_solver_request must be a list of:
6 elements for an RPY pose, or
7 elements for a Quaternion pose"""
return
if input_limb == "right" or input_limb == "left":
limb = input_limb
else:
print """Invalid Limb:
Input Limb to function: ik_solver_request must be a string:
'right' or 'left'"""
return
#request/response handling
#print quaternion_pose
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
raise
rospy.logerr("Service request failed: %r" % (error_message,))
def ikSolver(self,x,y,z,a,b,c,d):
rpy_pose = (x,y,z,a,b,c,d)
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/left/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 10.0)
ik_response = ik_service(ik_request)
if ik_response.isValid[0]:
print("Valid joint configuration found")
# convert response to joint position control dictionary
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
# move limb
self.limb.move_to_joint_positions(limb_joints)
rospy.sleep(1)
self.setTheta()
rospy.sleep(1)
return 0
else:
print "Failed"
return 1
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def to_stamped_pose(pose):
"""Take a xyzrpy or xyz qxqyqzqw pose and convert it to a stamped
pose (quaternion as orientation).
"""
stamped_pose = conversions.list_to_pose_stamped(pose, "base")
return stamped_pose
def callback(data):
x = data.data[0]
y = data.data[1]
print x,y
limb = baxter_interface.Limb('left')
pose = limb.endpoint_pose()
pose = limb.endpoint_pose()
xw,yw,zw = pose['position']
a,b,c,d = pose['orientation']
print xw,yw,zw
xw = xw + 0.01*x
yw = yw + 0.01*y
rpy_pose = (xw, yw, zw, 0, math.pi,0)
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + 'left'+ "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def solve_ik(input_limb, input_pose):
# print("IK requested for", input_limb)
if len(input_pose) != 6:
print("6 inputs required!")
return
if input_limb == "right" or input_limb == "left":
limb = input_limb
else:
print """Invalid Limb:
Input Limb to function: ik_solver_request must be a string:
'right' or 'left'"""
#convert the Eulers RPY into quaternion xyzw
quaternion_pose = conversions.list_to_pose_stamped(input_pose, "base")
#starting the IK request
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_requestmessage = SolvePositionIKRequest()
ik_requestmessage.pose_stamp.append(quaternion_pose)
# QUICK & DIRTY: MOLT THIS INTO AN TRY and EXCEPT handler!!
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_requestmessage)
if (ik_response.isValid[0]):
print("Jay! Valid joint solution found")
#convert response to JP control dict
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
return limb_joints
else:
print("FAILED: No valid joint configuration for this pose found")
def pose_limb(self, limb, pose):
#Convert the pose to a quaternion, then request an IK solution
quat_pose = conversions.list_to_pose_stamped(pose, "base")
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
header = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quat_pose)
#Wait for solution response
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
if ik_response.isValid[0]:
#Convert the solution to a joint position control dictionary and move
limb_joints = dict(
zip(ik_response.joints[0].name,
ik_response.joints[0].position))
baxter_interface.Limb(limb).move_to_joint_positions(limb_joints)
else:
sys.exit(
"pose_limb: A valid joint configuration could not be found")
#The result pose only matters for the right arm
if limb == "right":
quat_pose = baxter_interface.Limb("right").endpoint_pose()
#Update the manipulator's current pose
#Orientation shouldn't change, also endpoint_pose() returns orientation
#as a quaternion and that's annoying to use to update pose
self.curr_pose = pose
self.curr_pose[0] = quat_pose["position"][0]
self.curr_pose[1] = quat_pose["position"][1]
self.curr_pose[2] = quat_pose["position"][2]
def ik_solver_request(input_limb, input_pose):
print "IK solver request:"
# input error checking
if len(input_pose) == 6:
quaternion_pose = conversions.list_to_pose_stamped(input_pose, "base")
elif len(input_pose) == 7:
quaternion_pose = input_pose
else:
print """Invalid Pose List:
Input Pose to function: ik_solver_request must be a list of:
6 elements for an RPY pose, or
7 elements for a Quaternion pose"""
return
if input_limb == "right" or input_limb == "left":
limb = input_limb
else:
print """Invalid Limb:
Input Limb to function: ik_solver_request must be a string:
'right' or 'left'"""
return
# request/response handling
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
def to_stamped_pose(pose):
"""Take a xyzrpy or xyz qxqyqzqw pose and convert it to a stamped
pose (quaternion as orientation).
"""
stamped_pose = conversions.list_to_pose_stamped(pose, "base")
return stamped_pose
def get_random_pose(self, end_effector_link=""):
if len(end_effector_link) > 0 or self.has_end_effector_link():
return conversions.list_to_pose_stamped(
self._g.get_random_pose(end_effector_link),
self.get_planning_frame())
else:
raise MoveItCommanderException(
"There is no end effector to get the pose of")
def get_current_pose(self, end_effector_link=""):
""" Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """
if len(end_effector_link) > 0 or self.has_end_effector_link():
return conversions.list_to_pose_stamped(
self._g.get_current_pose(end_effector_link),
self.get_planning_frame())
else:
raise MoveItCommanderException(
"There is no end effector to get the pose of")
def transform(rpy_pose):
limb = 'left'
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(quaternion_pose)
ik_response = ik_service(ik_request)
# limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
# ik_response.joints[0].position = [s0, s1, e0, e1, w0, w1, w2], this is also the target point we want.
return(ik_response.joints[0].position)
def get_ik_solution(self,rpy_pose):
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + "right" + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 15.0) # 5改成了15
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def ik_move(self, limb, rpy_pose, print_result=False):
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message, ))
print "ERROR - baxter_ik_move - Failed to append pose"
def baxter_ik_move(self, limb, rpy_pose):
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def _move(self, rpy_pose, move=False):
print 'move:', rpy_pose
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + self.limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message, ))
return False
def inverse_kinematics(self, pose):
"""Determine the joint angles that provide a desired pose for the
robot"s end-effector."""
quaternion_pose = conversions.list_to_pose_stamped(pose, "base")
node = "ExternalTools/" + self.name + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: {}".format(error_message, ))
sys.exit("ERROR: Failed to append pose.")
def move_by_list(self, group, position_list):
pose = conversions.list_to_pose_stamped(position_list, "base")
group.set_pose_target(pose)
group.go()
rospy.sleep(0.5)
def to_stamped_pose(pose):
stamped_pose = conversions.list_to_pose_stamped(pose, "base")
return stamped_pose
def pose(self):
"""
@rtype: geometry_msgs.Pose
"""
return conversions.list_to_pose_stamped(self._robot._r.get_link_pose(self._name), self._robot.get_planning_frame())
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
# move limb
left.move_to_joint_positions(limb_joints)
else:
# display invalid move message on head display
#self.splash_screen("Invalid", "move")
# little point in continuing so exit with error message
print 'failed'
print "requested move =", rpy_pose
sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")
preDropAngles = left.joint_angles()
#go = raw_input('grasp?')
rpy_pose = (graspPointTorso[0]-0.03, graspPointTorso[1]-0.09, np.max((graspPointTorso[2]+0.1,0.06)), a,b,c,d) #0.06
print rpy_pose
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + 'left'+ "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
if ik_response.isValid[0]:
def streamDisp(data):
print 'size of data: ', sys.getsizeof(data)
global cloudy,ax,fig,graspPoint,save,kinectRotation,kinectTranslation,Trans
#data = do_transform_cloud(data,Trans)
left = baxter_interface.Limb('left')
grip = baxter_interface.Gripper('left')
if(not grip.calibrated()):
go = raw_input('grip cal?')
grip.calibrate()
grip.set_holding_force(40)
#print dir(data)
#print data.fields
#a = []
#qq = data.deserialize_numpy(data.data,cloudy)
#print type(cloudy)
#print type(qq)
#print qq.size
go = raw_input('find?')
print 'grasp parameters', graspPoint
x = graspPoint[0,0]
y = graspPoint[0,1]
theta = graspPoint[0,2]
x = int(x)
y = int(y)
gen2 = pc2.read_points(data, field_names = None, skip_nans=False, uvs=[[x,y]])
#print type(gen2)
in_data2 = next(gen2)
print'in data', in_data2
graspWorld = np.matrix([[in_data2[0]],[in_data2[1]],[in_data2[2]]])
#reflect = np.matrix([[0,1,0],[1,0,0],[0,0,1]])
#kinectRotation = np.dot(reflect,kinectRotation)
graspPointTorso = np.dot(kinectRotation,graspWorld)
graspPointTorso = np.add(graspPointTorso,kinectTranslation)
#graspPointTorso = np.add(graspWorld,kinectTranslation)
graspPointTorso = graspWorld
#print 'pointTorso',graspPointTorso[0],graspPointTorso[1],graspPointTorso[2]
#print 'rotated', np.dot(kinectRotation,graspWorld)
angles = left.joint_angles()
angles['left_w2'] = theta
left.move_to_joint_positions(angles)
pose = left.endpoint_pose()
xw,yw,zw = pose['position']
a,b,c,d = pose['orientation']
#print 'position',xw,yw,zw
#print 'orientation',a,b,c,d
#rpy_pose = (0.7*in_data[1]+0.6, 0.7*in_data[0]+0.06, in_data[2]-1.0, a,b,c,d)
rpy_pose = (graspPointTorso[0]-0.02, graspPointTorso[1]-0.07, graspPointTorso[2]+0.25, a,b,c,d)
#prePick = left.joint_angles()
print 'pose',rpy_pose
go = raw_input('move?')
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + 'left'+ "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
print 'trying new angle'
angles['left_w1'] = 1.2;
leftLimb.move_to_joint_positions(angles)
xw,yw,zw = pose['position']
a,b,c,d = pose['orientation']
rpy_pose = (graspPointTorso[0]-0.00, graspPointTorso[1]-0.07, graspPointTorso[2]+0.25, a,b,c,d)
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
def streamDisp(data):
print 'size of data: ', sys.getsizeof(data)
global cloudy, ax, fig, graspPoint, save, kinectRotation, kinectTranslation, Trans
#data = do_transform_cloud(data,Trans)
left = baxter_interface.Limb('left')
grip = baxter_interface.Gripper('left')
if (not grip.calibrated()):
go = raw_input('grip cal?')
grip.calibrate()
grip.set_holding_force(40)
#print dir(data)
#print data.fields
#a = []
#qq = data.deserialize_numpy(data.data,cloudy)
#print type(cloudy)
#print type(qq)
#print qq.size
go = raw_input('find?')
print 'grasp parameters', graspPoint
x = graspPoint[0, 0]
y = graspPoint[0, 1]
theta = graspPoint[0, 2]
x = int(x)
y = int(y)
gen2 = pc2.read_points(data,
field_names=None,
skip_nans=False,
uvs=[[x, y]])
#print type(gen2)
in_data2 = next(gen2)
print 'in data', in_data2
graspWorld = np.matrix([[in_data2[0]], [in_data2[1]], [in_data2[2]]])
#reflect = np.matrix([[0,1,0],[1,0,0],[0,0,1]])
#kinectRotation = np.dot(reflect,kinectRotation)
graspPointTorso = np.dot(kinectRotation, graspWorld)
graspPointTorso = np.add(graspPointTorso, kinectTranslation)
#graspPointTorso = np.add(graspWorld,kinectTranslation)
graspPointTorso = graspWorld
#print 'pointTorso',graspPointTorso[0],graspPointTorso[1],graspPointTorso[2]
#print 'rotated', np.dot(kinectRotation,graspWorld)
angles = left.joint_angles()
angles['left_w2'] = theta
left.move_to_joint_positions(angles)
pose = left.endpoint_pose()
xw, yw, zw = pose['position']
a, b, c, d = pose['orientation']
#print 'position',xw,yw,zw
#print 'orientation',a,b,c,d
#rpy_pose = (0.7*in_data[1]+0.6, 0.7*in_data[0]+0.06, in_data[2]-1.0, a,b,c,d)
rpy_pose = (graspPointTorso[0] - 0.02, graspPointTorso[1] - 0.07,
graspPointTorso[2] + 0.25, a, b, c, d)
#prePick = left.joint_angles()
print 'pose', rpy_pose
go = raw_input('move?')
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + 'left' + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message, ))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
print 'trying new angle'
angles['left_w1'] = 1.2
leftLimb.move_to_joint_positions(angles)
xw, yw, zw = pose['position']
a, b, c, d = pose['orientation']
rpy_pose = (graspPointTorso[0] - 0.00, graspPointTorso[1] - 0.07,
graspPointTorso[2] + 0.25, a, b, c, d)
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message, ))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
# move limb
left.move_to_joint_positions(limb_joints)
else:
# display invalid move message on head display
#self.splash_screen("Invalid", "move")
# little point in continuing so exit with error message
print 'failed'
print "requested move =", rpy_pose
sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")
preDropAngles = left.joint_angles()
#go = raw_input('grasp?')
rpy_pose = (graspPointTorso[0] - 0.03, graspPointTorso[1] - 0.09,
np.max((graspPointTorso[2] + 0.1, 0.06)), a, b, c, d) #0.06
print rpy_pose
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + 'left' + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message, ))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
if ik_response.isValid[0]:
def get_random_pose(self, end_effector_link=""):
if len(end_effector_link) > 0 or self.has_end_effector_link():
return conversions.list_to_pose_stamped(self._g.get_random_pose(end_effector_link), self.get_planning_frame())
else:
raise MoveItCommanderException("There is no end effector to get the pose of")
def get_current_pose(self, end_effector_link=""):
""" Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """
if len(end_effector_link) > 0 or self.has_end_effector_link():
return conversions.list_to_pose_stamped(self._g.get_current_pose(end_effector_link), self.get_planning_frame())
else:
raise MoveItCommanderException("There is no end effector to get the pose of")
def to_stamped_pose(pose): stamped_pose = conversions.list_to_pose_stamped(pose, "base") return stamped_pose
def move_by_list(self, group, position_list):
pose = conversions.list_to_pose_stamped(position_list,"base")
group.set_pose_target(pose)
group.go()
rospy.sleep(0.5)
