Read joint angle


#1

Hi all, I’m Marco, I’m totally new to the platform.
I’d like to execute a task where iCub moves its elbow joint until he reaches a certain angle (or equivalently position). The problem is: even though I’m capable of sending messages to the elbow with the command

r_elbow.send_message(std_msgs.msg.Float64(1.))

I don’t know how to read the current joint value.
Refering to iCub tutorial baseball solutions, I found a transfer function called joints_state_passthrough that I suppose to get the joints value:

from sensor_msgs.msg import JointState
@nrp.MapRobotSubscriber(“joints”, Topic("/robot/joints", JointState))
@nrp.Neuron2Robot(Topic(’/joint_states’, JointState))
def joint_states_passthrough(t, joints):
return joints.value

How can I have access to a particular joint position value (e.g.: to print it on the log console)?

Thank you a lot,
Marco


#2

Dear Marco,

The transfer function set_eyepos.py located in $HBP/Experiments/visual_tracking_icub (online, this corresponds to the “Holodeck iCub Visual Tracking experiment”) contains an example:

from sensor_msgs.msg import JointState
@nrp.MapVariable("eye_position", initial_value=None, scope=nrp.GLOBAL)
@nrp.MapRobotSubscriber("joints", Topic("/icub/joints", JointState))
@nrp.Robot2Neuron()
def set_eyepos(t, eye_position, joints):
    joints = joints.value
    if joints is not None:
        eye_position.value = joints.position[joints.name.index('eye_version')]

In the above function, we subscribe to the “/icub/joints” topic (all the joints values of the icub robots) and read the “eye_version” joint’s value in order to set a variable named eye_position.

Best regards,
Luc


#3

Ok, great, thank you Luc!
Best regards,
Marco