"""
Example of moving the robot in cartesian space to push the ball. It is more robust than the pure joint control.
:Author: Lukas Rustler
"""
try:
from icub_pybullet.pycub import pyCub
except:
import sys
import os
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))))
from icub_pybullet.pycub import pyCub
from icub_pybullet.utils import Pose
[docs]def push_the_ball(client):
"""
Function to move the ball with cartesian control. The robot is moved 15cm lower and 10cm closer and the moved left to
push the ball.
"""
# Get current pose
cur_pose = client.end_effector.get_position()
# Copy, the pose and go 15cm lower and 10cm closer to the robot
new_pose = Pose(cur_pose.pos, cur_pose.ori)
new_pose.pos[2] -= 0.15
new_pose.pos[0] -= 0.1
# move
client.move_cartesian(new_pose)
# get current pose
pose = client.end_effector.get_position()
# assign straight to it to move hand left
pose.pos[1] -= 0.2
# move; do not wait for completion; and move a bit faster
client.move_cartesian(pose, wait=False, velocity=2)
# wait manually
while not client.motion_done():
client.update_simulation()
"""
Wait could be also achieved with:
client.wait_motion_done()
"""
client.logger.info("Moved the ball!")
[docs]def main():
# load the robot with correct world/config
client = pyCub(config="with_ball.yaml")
push_the_ball(client)
# just wait until the gui is closed
while client.is_alive():
client.update_simulation()
if __name__ == "__main__":
main()