pink/configuration.py: use the more generic frames instead of bodies

[proyan]

Fixes https://github.com/tasts-robots/pink/issues/48

I tried with a sample script to make sure I'm not breaking anything, and the results for BODY and OP_FRAME are consistent.

import pink
import pinocchio
from robot_descriptions.loaders.pinocchio import load_robot_description
import numpy as np
robot = load_robot_description("ur10_description")

# Frame Details:
frame_name = "ee_frame"
joint_name = robot.model.names[-1]
parent_joint = robot.model.getJointId(joint_name)
parent_frame = robot.model.getFrameId(joint_name)
placement = pinocchio.SE3.Identity()

ee_frame = robot.model.addFrame(
    pinocchio.Frame(frame_name,
                    parent_joint,
                    parent_frame,
                    placement,
                    pinocchio.FrameType.OP_FRAME
                    )
)
robot.data = pinocchio.Data(robot.model)
low = robot.model.lowerPositionLimit
high = robot.model.upperPositionLimit
robot.q0 = pinocchio.neutral(robot.model)
# Task Details:
np.random.seed(0)

q_final = np.array([np.random.uniform(low=low[i], high=high[i], size=(1,))[0] for i in range(6)])
pinocchio.forwardKinematics(robot.model, robot.data, q_final)

target_pose = robot.data.oMi[parent_joint]
ee_task = pink.tasks.BodyTask(frame_name, [1.0, 1.0, 1.0], [1.0, 1.0, 1.0])
ee_task.set_target(target_pose)
tasks = [ee_task]

#IK:
dt = 1e-2
damping = 1e-8
niter = 10000
solver = "quadprog"

pink_configuration = pink.Configuration(
    robot.model, robot.data, robot.q0
)

for i in range(niter):
    dv = pink.solve_ik(
        pink_configuration,
        [ee_task],  # , config_task],
        dt=dt,
        damping=damping,
        solver=solver,
    )
    q_out = pinocchio.integrate(robot.model, pink_configuration.q, dv * dt)
    pink_configuration = pink.Configuration(robot.model, robot.data, q_out)
    pinocchio.updateFramePlacements(robot.model, robot.data)
    err = ee_task.compute_error(pink_configuration)
    print(i, err)
    if np.linalg.norm(ee_task.compute_error(pink_configuration)) < 1e-8:
        break

[proyan]

For the naming, the BodyTask might be renamed to FrameTask, but I tried not to break the API :)

[stephane-caron]

LGTM. Your example is informative too :+1: Do you want to add it e.g. as examples/arm_with_op_frame.py?

It would also be good practice to add a unit test with an OP_FRAME (the first part of your example would work as is). This would enact those frames as part of the API and prevent future regressions. Could you add such a test?

For the naming, the BodyTask might be renamed to FrameTask, but I tried not to break the API :)

That's totally OK :ok_hand: Pink is still at the pre-1.0 stage. I think FrameTask would be a better name after this update. If you want we can rename it in this PR, or I can take care of the renaming in a follow-up PR.

[proyan]

I've changed some instances of the Body to Frame that I could find (this PR is now going much deeper than initially expected :) ) I don't think I have completely cleaned the occurrences, so you might still have some work. Other than that, there is now the unittest and the example as well in the PR.

[stephane-caron]

LGTM, thank you for taking care of all changes!

The CI broke because of a cmeel dependency, currently waiting on https://github.com/stack-of-tasks/pinocchio/issues/1937 to be fixed. Alternatively you can add eigenpy==2.9.2 to all test environments in tox.ini to unlock this (I would take care of reverting it later).

[proyan]

I've updated the CI, but I think you need to approve the workflow

[stephane-caron]

Passed all tests :+1: I will fix the linter error after the merge.

Thank you for this contribution :smiley: