hsri

cup_demo.py

from hsrb_interface import geometry

import hsrb_interface
from hsrb_interface import geometry

robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
tts = robot.get('default_tts')
tts.language = 1
tts.say("Hello")

_BOTTLE_TF='ar_marker/4'

# cup_to_hand = geometry.pose(x=0.000, y=-0.001, z=0.144, ek=1.521)
cup_to_hand = geometry.pose(y=-0.17, ei=-1.521)

whole_body.move_end_effector_pose(cup_to_hand, _BOTTLE_TF)

old_hand = whole_body.get_end_effector_pose()
# cup_to_hand2 = geometry.pose(y=-0.12, ei=-1.521)

old_hand.pos.z = old_hand.pos.z - 0.03
new_hand = geometry.pose(x=old_hand.pos.z, y=old_hand.pos.z, z= old_hand.pos.

whole_body.move_end_effector_pose(old_hand, 'base_link')