move baxter end effector position regardless of orientation
I downloaded these codes from the link https://www.google.co.uk/url?sa=t&rct...
first code ik_solver.py import argparse import sys
import rospy
from geometry_msgs.msg import (
PoseStamped,
Pose,
Point,
Quaternion,
)
from std_msgs.msg import Header
from baxter_core_msgs.srv import (
SolvePositionIK,
SolvePositionIKRequest,
)
def ik_solve(limb, pos, orient):
#~ rospy.init_node("rsdk_ik_service_client")
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
print "iksvc: ", iksvc
print "ikreq: ", ikreq
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
str(limb): PoseStamped(header=hdr,
pose=Pose(position=pos, orientation=orient))}
ikreq.pose_stamp.append(poses[limb])
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 1
if (resp.isValid[0]):
print("SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
print limb_joints
return limb_joints
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return -1
The second code movement_commands.py
import baxter_interface
import baxter_external_devices
import rospy
import ik_solver
from geometry_msgs.msg import (
Point,
Quaternion,
)
# Moves relative to current Position
def moveRel(x,y,z):
pose = left.endpoint_pose()
print "current pose: ", pose
curX = pose["position"].x
curY = pose["position"].y
curZ = pose["position"].z
newX = curX + x
newY = curY + y
newZ = curZ + z
loc = Point(newX,newY,newZ)
print "loc: ", loc
faceDown = Quaternion( 0.0, 0.0, 0.5, 0.0)
#print "orient: ", faceDown
limb_joints = ik_solver.ik_solve('left', loc, faceDown)
prev = loc
if limb_joints != -1:
print "limb_joints: ", limb_joints
print "moving arm to limb_joints joints"
left.move_to_joint_positions(limb_joints)
# Takes a Point()
def moveTo(goal):
loc = goal
faceDown = Quaternion( 0.0, 0.0, 0.5, 0.0)
#print "loc: ", loc
#print "orient: ", orient[1]
limb_joints = ik_solver.ik_solve('left', loc, faceDown)
prev = loc
if limb_joints != -1:
#print "limb_joints: ", limb_joints
#print "moving arm to goal limb_joints joints"
print " "
print " "
print " "
left.move_to_joint_positions(limb_joints)
rospy.init_node('move_left_hand')
rs = baxter_interface.RobotEnable()
rs.enable()
left = baxter_interface.Limb('left')
leftGripper = baxter_interface.Gripper('left')
pose = left.endpoint_pose()
b = True
pos = pose.popitem()
orient = pose.popitem()
prev = pos[1]
left.set_joint_position_speed(1.0)
print "-----------------------------------"
pose = left.endpoint_pose()
print "current pose: ", pose
s = raw_input("Press q to run ... ")
if s == 'q':
moveTo(Point(-0.008758931559827073, 0.4037057092838424, 1.1405580898861356))
print left.endpoint_pose()
The second code movement_commands.py has a function moveTo where you put the position of the end effector and it moves there but you also have to put the orientation which is a quaternion. I don’t completely understand how the orientation works so I want to ask if there is a way to only put the position of the I want the end effector to be at without having to use the orientation.