"""
A Franka Panda moves using delta end effector pose control.
This script contains examples of:
- IK calculations.
- Joint movement by setting joint target positions.
"""
from os.path import dirname, join, abspath
from pyrep import PyRep
from pyrep.robots.arms.panda import Panda
SCENE_FILE = join(dirname(abspath(__file__)), 'scene_panda_reach_target.ttt')
DELTA = 0.01
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
agent = Panda()
starting_joint_positions = agent.get_joint_positions()
pos, quat = agent.get_tip().get_position(), agent.get_tip().get_quaternion()
def move(index, delta):
pos[index] += delta
new_joint_angles = agent.solve_ik_via_jacobian(pos, quaternion=quat)
agent.set_joint_target_positions(new_joint_angles)
pr.step()
[move(2, -DELTA) for _ in range(20)]
[move(1, -DELTA) for _ in range(20)]
[move(2, DELTA) for _ in range(10)]
[move(1, DELTA) for _ in range(20)]
pr.stop()
pr.shutdown()