def get_panda_ik(panda: gym_ignition_environments.models.panda.Panda,
optimized_joints: List[str]) -> \
inverse_kinematics_nlp.InverseKinematicsNLP:
# Create IK
ik = inverse_kinematics_nlp.InverseKinematicsNLP(
urdf_filename=panda.get_model_file(),
considered_joints=optimized_joints,
joint_serialization=panda.joint_names())
# Initialize IK
ik.initialize(verbosity=1,
floating_base=False,
cost_tolerance=1E-8,
constraints_tolerance=1E-8,
base_frame=panda.base_frame())
# 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()))
# 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)
return ik
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.ik_joints = [
j.name()
for j in self.model.joints()
if j.type is not scenario_core.JointType_fixed and "finger" not in j.name()
]
# Initialize Panda in Ignition
assert (
self.get_joint(joint_name="panda_finger_joint1")
.to_gazebo()
.set_max_generalized_force(max_force=500.0)
)
assert (
self.get_joint(joint_name="panda_finger_joint2")
.to_gazebo()
.set_max_generalized_force(max_force=500.0)
)
# Initialize IK
ik_joints = self.ik_joints
ik = inverse_kinematics_nlp.InverseKinematicsNLP(
urdf_filename=self.get_model_file(),
considered_joints=ik_joints,
joint_serialization=self.joint_names(),
)
ik.initialize(
verbosity=0,
floating_base=False,
cost_tolerance=1e-8,
constraints_tolerance=1e-8,
base_frame=self.base_frame(),
)
ik.set_current_robot_configuration(
base_position=np.array(self.base_position()),
base_quaternion=np.array(self.base_orientation()),
joint_configuration=self.home_position,
)
ik.add_target(
frame_name="end_effector_frame",
target_type=inverse_kinematics_nlp.TargetType.POSE,
as_constraint=False,
)
ik.solve() # warm up IK
self.ik = ik
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()