sawyer inverse kinematics


lr101095

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

Link to comment
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?

Link to comment
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

Link to comment
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. 

Link to comment
Share on other sites

Archived

This topic is now archived and is closed to further replies.