lr101095

sawyer inverse kinematics

Recommended Posts

trying to write a program that subscribes to the endpoint states of sawyer through rostopics. i've opted to subscribe to each individual endpoint_state value.

my program is based around ik_service_client, hence why i decided to subscribe to the individual values.

I have the following:

    sub_p1 = rospy.Subscriber("/robot/limb/right/endpoint_state/pose/position/x", String)
    sub_p2 = rospy.Subscriber("/robot/limb/right/endpoint_state/pose/position/y", String)
    sub_p3 = rospy.Subscriber("/robot/limb/right/endpoint_state/pose/position/z", String)
    
    sub_q1 = rospy.Subscriber("/robot/limb/right/endpoint_state/pose/orientation/x", String)
    sub_q2 = rospy.Subscriber("/robot/limb/right/endpoint_state/pose/orientation/y", String)
    sub_q3 = rospy.Subscriber("/robot/limb/right/endpoint_state/pose/orientation/z", String)
    sub_q0 = rospy.Subscriber("/robot/limb/right/endpoint_state/pose/orientation/w", String)

    poses = {'right': PoseStamped(
                    header = hdr,
                    pose = Pose(
                    position = Point(
                            x = sub_p1,
                            y = sub_p2,
                            z = sub_p3,
                        ),
                    orientation = Quaternion(
                            x = sub_q1,
                            y = sub_q2,
                            z = sub_q3,
                            w = sub_q0,
                        ),
                    ),
                ),
            }

However, as a test, i print "poses" and i get the following output:

pose:
  position:
    x: <rospy.topics.Subscriber object at 0x7fe9f9c31c50>
    y: <rospy.topics.Subscriber object at 0x7fe9f9c31d50>
    z: <rospy.topics.Subscriber object at 0x7fe9f9c31d90>
  orientation:
    x: <rospy.topics.Subscriber object at 0x7fe9f9bd7050>
    y: <rospy.topics.Subscriber object at 0x7fe9f9c31e10>
    z: <rospy.topics.Subscriber object at 0x7fe9f9bd7710>
    w: <rospy.topics.Subscriber object at 0x7fe9f9bd79d0>}

How am i able to print/access the actual data so i have an output of actual numbers that i can then pass on to perform inverse kinematics?

I'm not sure if i'm using the correct data type (i.e. String or otherwise) to pass on the values.

any corrections, suggestions or solutions would be much appreciated.

- Luke

Edited by lr101095

Share this post


Link to post
Share on other sites

ROS subscribers run using callbacks http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber(python)

Example for getting and using the endpoint state

import rospy
from intera_core_msgs.msg import EndpointState
from geometry_msgs.msg import Pose

global ee_pose

def ee_cb(msg)
	global ee_pose
	ee_pose = msg.pose


if __name__=='__main__':
	global ee_pose
	ee_pose = Pose()
	rospy.init_node('ee_node')
	rospy.Subscriber("/robot/limb/right/endpoint_state", EndPointState, callback=ee_cb)
	r = rospy.Rate(100)

	while not rospy.is_shutdown():
		print ee_pos  # do whatever you need with the ee_pose
		print ee_pos.position.x # for example
		r.sleep()

The question though is why do you then need to perform IK on the endpoint state, when the joint positions are published on the /robot/joint_states topic?

Share this post


Link to post
Share on other sites

Hi Senaa,

I was trying to understand the mechanics of the ik_service_client within Intera. For me, i was using endpoint_states as a test input so that i can double check if i'm using ik_service_client correctly since i know what joint states i should be expecting.

I also wanted to ask how to subscribe to endpoint_states for a later application. The way you suggested, would that allow me to extract position and quaternion?

- Luke

Share this post


Link to post
Share on other sites

Ah ok I see, and yes this gives you position and orientation data as the ee_pose variable type is geometry_msgs/Pose

Eg how to access the x position of the pose and w component of the orientation:

ee_pos.position.x 
ee_pos.orientation.w

the other elements are retrieved in a similar way. 

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now