Ask Your Question
1

move baxter end effector position regardless of orientation

asked 2017-04-10 08:15:52 -0500

Glass_Eater gravatar image

updated 2017-04-11 08:09:01 -0500

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.

edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
0

answered 2017-04-10 13:01:14 -0500

Airuno2L gravatar image

It is easier if you input a roll pitch yaw and convert that into a quaternion. The tf.transformations library has the function quaternion_from_euler that does just that.

Being able to input just a position, without an orientation requires a special kind of inverse kinematics solver, I'm not sure what kind they're using so I'm not sure if it's (easily) possible.

edit flag offensive delete link more
0

answered 2017-04-10 22:35:53 -0500

Try to use the move it planner, and perform a cartesian to robot joint angles transformation. That would give you your destination and origin position as you need in whichever orientation. Alternatively, you can also use the move it planner and simulate your desired location and orientation, and copy the points from there into your program.

It is ideal to use a transformation from cartesian to robot joint angles. If you are sending raw points it will always have every joint moving, and you cannot really isolate each joint movements.

Maybe you can provide your code location ( the link you have given takes me to the brr google group), and then the actual intent of the code could be realized. Also, I would always prefer using moveit library (just a personal choice) as it gives you access to all sorts of planners including IK.

Also what is your application intent?

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-04-10 08:15:52 -0500

Seen: 598 times

Last updated: Apr 11 '17