def add_panda_controller(panda: gym_ignition_environments.models.panda.Panda,
controller_period: float) -> None:
# Set the controller period
assert panda.set_controller_period(period=controller_period)
# Increase the max effort of the fingers
panda.get_joint(joint_name="panda_finger_joint1").to_gazebo(). \
set_max_generalized_force(max_force=500.0)
panda.get_joint(joint_name="panda_finger_joint2").to_gazebo(). \
set_max_generalized_force(max_force=500.0)
# Insert the ComputedTorqueFixedBase controller
assert panda.to_gazebo().insert_model_plugin(
*controllers.ComputedTorqueFixedBase(
kp=[100.0] * (panda.dofs() - 2) + [10000.0] * 2,
ki=[0.0] * panda.dofs(),
kd=[17.5] * (panda.dofs() - 2) + [100.0] * 2,
urdf=panda.get_model_file(),
joints=panda.joint_names(),
).args())
# Initialize the controller to the current state
assert panda.set_joint_position_targets(panda.joint_positions())
assert panda.set_joint_velocity_targets(panda.joint_velocities())
assert panda.set_joint_acceleration_targets(panda.joint_accelerations())
def __init__(self, **kwargs):
self.home_position = np.array(
(0, -0.785, 0, -2.356, 0, 1.571, 0.785, 0.03, 0.03))
super().__init__(**kwargs)
# Constraints
# joint constraints (units in rad, e.g. rad/s for velocity)
# TODO: check the values of the fingers, these are all guesses
self.max_position = np.array((2.8973, 1.7628, 2.8973, 0.0698, 2.8973,
3.7525, 2.8973, 0.045, 0.045))
self.min_position = np.array(
(-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973,
-0.001, -0.001))
self.max_velocity = np.array(
(2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100, 0.3, 0.3))
self.min_velocity = -self.max_velocity
self.max_acceleration = np.array(
(15, 7.5, 10, 12.5, 15, 20, 20, 10, 10), dtype=np.float_)
self.min_acceleration = -self.max_acceleration
self.max_jerk = np.array(
(7500, 3750, 5000, 6250, 7500, 10000, 10000, 10000, 10000),
dtype=np.float_)
self.min_jerk = -self.max_jerk
self.max_torque = np.array((87, 87, 87, 87, 12, 12, 12, 12, 12),
dtype=np.float_)
self.min_torque = -self.max_torque
self.max_rotatum = np.array([1000] * 9)
self.min_rotatum = -self.max_rotatum
# tool constraints
self.max_tool_velocity = 1.7 # m/s
self.max_tool_acceleration = 13 # m/s
self.max_tool_jerk = 6500 # m/s
self.max_tool_angular_velocity = 2.5 # rad/s
self.max_tool_angular_acceleration = 25 # rad/s
self.max_tool_angular_jerk = 12500 # rad/s
# ellbow constraints (in rad)
# This is in the docs, but I'm not sure how to interpret it. Perhaps it
# refers to null-space motion?
# https://frankaemika.github.io/docs/control_parameters.html
self.max_ellbow_velocity = 2.175
self.max_ellbow_acceleration = 10
self.max_ellbow_jerk = 5000
panda = self.model
panda.to_gazebo().enable_self_collisions(True)
# Insert the ComputedTorqueFixedBase controller
assert panda.to_gazebo().insert_model_plugin(
*controllers.ComputedTorqueFixedBase(
kp=[100.0] * (self.dofs - 2) + [10000.0] * 2,
ki=[0.0] * self.dofs,
kd=[17.5] * (self.dofs - 2) + [100.0] * 2,
urdf=self.get_model_file(),
joints=self.joint_names(),
).args())
def test_computed_torque_fixed_base(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
step_size = gazebo.step_size()
# Get the default world
world = gazebo.get_world()
# Insert the physics
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# Get the panda urdf
panda_urdf = gym_ignition_models.get_model_file("panda")
# Insert the panda arm
model_name = "panda"
assert world.insert_model(panda_urdf, core.Pose_identity(), model_name)
# import time
# gazebo.gui()
# time.sleep(3)
# gazebo.run(paused=True)
# Get the model
panda = world.get_model(model_name).to_gazebo()
# Set the controller period
panda.set_controller_period(step_size)
# Insert the controller
assert panda.insert_model_plugin(*controllers.ComputedTorqueFixedBase(
kp=[10.0] * panda.dofs(),
ki=[0.0] * panda.dofs(),
kd=[3.0] * panda.dofs(),
urdf=panda_urdf,
joints=panda.joint_names(),
).args())
# Set the references
assert panda.set_joint_position_targets([0.0] * panda.dofs())
assert panda.set_joint_velocity_targets([0.0] * panda.dofs())
assert panda.set_joint_acceleration_targets([0.0] * panda.dofs())
joints_no_fingers = [
j for j in panda.joint_names() if j.startswith("panda_joint")
]
nr_of_joints = len(joints_no_fingers)
assert nr_of_joints > 0
# Reset the joints state
q0 = [np.deg2rad(45)] * nr_of_joints
dq0 = [0.1] * nr_of_joints
assert panda.reset_joint_positions(q0, joints_no_fingers)
assert panda.reset_joint_velocities(dq0, joints_no_fingers)
assert gazebo.run(True)
assert panda.joint_positions(joints_no_fingers) == pytest.approx(q0)
assert panda.joint_velocities(joints_no_fingers) == pytest.approx(dq0)
# Step the simulator for a couple of seconds
for _ in range(3000):
gazebo.run()
# Check that the the references have been reached
assert panda.joint_positions() == pytest.approx(
panda.joint_position_targets(), abs=np.deg2rad(1))
assert panda.joint_velocities() == pytest.approx(
panda.joint_velocity_targets(), abs=0.05)
# Apply an external force
assert (panda.get_link("panda_link4").to_gazebo().apply_world_force(
[100.0, 0, 0], 0.5))
for _ in range(4000):
assert gazebo.run()
# Check that the the references have been reached
assert panda.joint_positions() == pytest.approx(
panda.joint_position_targets(), abs=np.deg2rad(1))
assert panda.joint_velocities() == pytest.approx(
panda.joint_velocity_targets(), abs=0.05)
def test_inverse_kinematics(
default_world: Tuple[scenario_gazebo.GazeboSimulator,
scenario_gazebo.World]):
# Get the simulator and the world
gazebo, world = default_world
# Get the robot
panda = models.panda.Panda(world=world)
gazebo.run(paused=True)
# Control all the joints
controlled_joints = panda.joint_names()
# Set the controller period
assert panda.set_controller_period(gazebo.step_size())
assert panda.controller_period() == gazebo.step_size()
# Insert the ComputedTorqueFixedBase controller
assert panda.to_gazebo().insert_model_plugin(
*controllers.ComputedTorqueFixedBase(
kp=[20.0] * panda.dofs(),
ki=[0.0] * panda.dofs(),
kd=[5.0] * panda.dofs(),
urdf=panda.get_model_file(),
joints=controlled_joints,
).args())
# Create the IK object
ik = inverse_kinematics_nlp.InverseKinematicsNLP(
urdf_filename=panda.get_model_file(),
considered_joints=controlled_joints,
joint_serialization=panda.joint_names())
# Initialize IK
ik.initialize(verbosity=1, floating_base=False, cost_tolerance=1E-8)
# There should be no active targets
assert len(ik.get_active_target_names()) == 0
# Add the cartesian target of the end effector
end_effector = "end_effector_frame"
ik.add_target(frame_name=end_effector,
target_type=inverse_kinematics_nlp.TargetType.POSE,
as_constraint=False)
assert set(ik.get_active_target_names()) == {end_effector}
assert ik.get_target_data(target_name=end_effector).type == \
inverse_kinematics_nlp.TargetType.POSE
# Desired EE position
target_ee_position = np.array([0.5, -0.3, 1.0])
target_ee_quaternion = np.array([0.0, 1.0, 0.0, 0.0])
# Update the target transform
ik.update_transform_target(target_name=end_effector,
position=target_ee_position,
quaternion=target_ee_quaternion)
# Check that the target transform was stored
assert isinstance(
ik.get_target_data(target_name=end_effector).data,
inverse_kinematics_nlp.TransformTargetData)
assert ik.get_target_data(target_name=end_effector).data.position == \
pytest.approx(target_ee_position)
assert ik.get_target_data(target_name=end_effector).data.quaternion == \
pytest.approx(target_ee_quaternion)
# Set the current configuration
ik.set_current_robot_configuration(
base_position=np.array(panda.base_position()),
base_quaternion=np.array(panda.base_orientation()),
joint_configuration=np.array(panda.joint_positions()))
# Solve the IK problem
ik.solve()
# Get the full solution
ik_solution = ik.get_full_solution()
# Validate the solution
assert ik_solution.joint_configuration.size == len(controlled_joints)
assert ik_solution.base_position == pytest.approx(np.array([0.0, 0, 0]))
assert ik_solution.base_quaternion == pytest.approx(
np.array([1.0, 0, 0, 0]))
# Set the desired joint configuration
assert panda.set_joint_position_targets(ik_solution.joint_configuration,
controlled_joints)
assert panda.set_joint_velocity_targets([0.0] * len(controlled_joints))
assert panda.set_joint_acceleration_targets([0.0] * len(controlled_joints))
for _ in range(5_000):
gazebo.run()
# assert world.set_physics_engine(scenario_gazebo.PhysicsEngine_dart)
# ---- initialize panda ----
# spawn
panda = gym_ignition_environments.models.panda.Panda(world=world,
position=[0.2, 0, 1.025])
end_effector = panda.get_link("end_effector_frame")
gazebo.gui()
gazebo.run(paused=True) # needs to be called before controller initialization
# Insert the ComputedTorqueFixedBase controller
assert panda.to_gazebo().insert_model_plugin(
*controllers.ComputedTorqueFixedBase(
kp=[100.0] * (panda.dofs() - 2) + [10000.0] * 2,
ki=[0.0] * panda.dofs(),
kd=[17.5] * (panda.dofs() - 2) + [100.0] * 2,
urdf=panda.get_model_file(),
joints=panda.joint_names(),
).args())
assert panda.set_joint_position_targets(panda.joint_positions())
assert panda.set_joint_velocity_targets(panda.joint_velocities())
assert panda.set_joint_acceleration_targets(panda.joint_accelerations())
home_position = np.array(end_effector.position())
panda_ctrl = LinearJointSpacePlanner(panda,
control_frequency=gazebo.step_size())
ik_joints = [
j.name() for j in panda.joints()
if j.type is not scenario_core.JointType_fixed