A first approach to impedance control

[PeterBowman]

TEO has recently learned to behave like a spring (boing, boing), or at least with one of its upper limbs rather than using the whole body. So far, it integrates Hooke's law to obtain the resulting external force to be applied on the TCP while a human operator drives one of its hands off an initial pose during gravity compensation control. I fact, all it does is to multiply the displacement between such pose and its current position by a stiffness. Then, the contribution of the resulting vector of forces is used to perform inverse dynamics and, adding to the computed torques needed to keep the arm in place (gravity compensation), another contribution helps to make the TCP travel back with a spring-like effect.

Much to TEO's disgrace, a suitable value of stiffness has been estimated through trial and error to range between 100-150. Anything higher than that requires extra care and good reflexes.

At the time of writing, only linear forces are taken into account in the current implementation: https://github.com/roboticslab-uc3m/kinematics-dynamics/compare/6e78a53...b8ed4ca. Torques (in cartesian space) could be easily introduced thanks to ICartesianSolver::poseDiff, but keep in mind that torsional stiffness should be used in such cases instead.

We observed that it's hard to get back the initial pose after releasing the arm. Also, if higher displacements and stiffness values are applied, it behaves like an oscillator, hence some sort of damping effect could be of use.

In addition to the previous ideas, the torques required to undo the displacement (in cartesian space, considering a sole external force applied on the TCP) might be obtained through the same Hooke's law, but now assuming it behaves like several torsion springs instead. That is, measure the angles (one per joint) between initial and current pose, then use the resulting torque in the outcome of inverse dynamics.

[PeterBowman]

TODOs:

• investigate iCUB/YARP usage of the position/velocity impedance mode from yarp::dev::IControlMode (ref, deprecated long ago)
  • perhaps superseded by yarp::dev::InteractionMode
• investigate yarp::dev::ImpedanceControl
• what about friction? https://github.com/robotology-playground/torque-control-params-estimation
• how to combine gravity compensation and internal friction forces? http://roboticslab.uc3m.es/roboticslab/book/advances-intelligent-systems-and-computing-vol-418 (thanks, @jgvictores)

[PeterBowman]

Watch and limit velocities? See https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/163.

[PeterBowman]

I'm going to transfer this issue from the kinematics-dynamics repo to yarp-devices. I believe this problem should be tackled in the joint space, not in the cartesian space.

First, the modifications to BasicCartesianControl proposed at https://github.com/roboticslab-uc3m/kinematics-dynamics/compare/6e78a53...b8ed4ca are a hackish way to demonstrate impedance control, and nothing more than that. It has been implemented on top of the gravity compensation command for simplicity, but it defeats the real purpose of gcmp. In fact, if I were to implement a spring-like behavior while maintaining position, I'd rather configure the manipulator in position control mode and enable the compliant behavior via IInteractionMode.

Secondly, imposing compliance on a per-joint manner (joint space) is quite equivalent to simulating a spring-like response on external forces applied on the TCP (cartesian space), which need to be translated back to joint space via inverse dynamics anyway. Quite, but not really, since our solution doesn't account for forces applied elsewhere, i.e. on intermediate joints ant links. Besides, we should account for changes in orientation as well (we didn't implement this on the testing branch and almost made the robot collide with itself when the arm started to oscillate).

In conclusion, although it might be of interest to revisit cartesian-space impedance control at some point, it is more reasonable to have a working joint-space implementation under the hood (also, that should be way easier).

[PeterBowman]

There are two relevant YARP interfaces related to compliant/impedance control:

• yarp::dev::IInteractionMode: configure either stiff or compliant mode
• yarp::dev::IImpedanceControl: configure parameters for compliant mode (stiffness, damping, offset)

So far we implemented neither and the TechnosoftIpos device assumes default stiff mode.

This is how compliance is achieved in the iCub robot (wiki, pdf):

Remarks:

• IPositionDirect seems pretty straightforward.
• IPositionControl requires an underlying trajectory generator to mimic the one that governs profile position mode in the iPOS.
• IVelocityControl would either require integration (as in the iCub) or a generator similar to iPOS profile velocity mode.

Note torque control mode is always selected under the hood for compliant motion. I believe the iCub is always commanded with torques, then hand-crafted control layers atop generate position setpoints (see iCub::ctrl). We used to take advantage of real-time position control embedded in the iPOS for stiff motion (makes sense to avoid reinventing the wheel and ending up with a square one). It might be cumbersome to select e.g. IPositionControl mode and switch to ITorqueControl thereafter because there must be a strong dependency between IControlMode and IInteractionMode in our case, but I don't think it could be done better.

[PeterBowman]

GazeboYarpControlBoardDriver works with non-cascading position and velocity PIDs that translate to torque commands. Just for reference, impedance control has been implemented as a separate PD compliant controller that replaces the stiff PID. P and D coefficients are stiffness and damping values, respectively: ref.

Perhaps the internal PID controllers in our iPOS drives could be configured on the fly... (while also accounting for feed-forward inputs).

[PeterBowman]

RPC commands for the IImpedanceControl interface:

• [set] [iimp] [ipr] joint_id (stiffness damping) (also with [get], returns list of 2 elements)
• [set] [iimp] [iof] joint_id (offset) (also with [get], returns list of 1 element)
• [get] [iimp] [llim] joint_id (returns list of 4 elements: min/max stiffness, min/max damping)

RPC commands for the IInteractionMode interface:

• [set] [intm] [mode] joint_id mode (also with [get], mode is either [stif], [comp] or [unkn])
• [set] [intm] [modg] n_joints (joint_id1 joint_id2...) (mode1 mode2...) (also with [get])
• [set] [intm] [mods] n_joints (mode1 mode2...) (also with [get])

[PeterBowman]

The Gazebo-YARP implementation of the PD impedance controller is here, albeit in a slightly different form as the one depicted in https://github.com/roboticslab-uc3m/yarp-devices/issues/258#issuecomment-786629155. The first component has its sign reversed:

ref_torque = - P_gain * (q - ref_q) - D_gain * qdot + torque_offset

torque_offset has a getter/setter in IImpedanceControl as well.

Regarding real robot firmware:

• https://github.com/robotology/icub-firmware/blob/e9e5167690449b18480f809fb3243f800d97717a/motorControllerDsp56f807/common_source_code/pid.c#L435-L494
• https://github.com/robotology/icub-firmware/blob/e9e5167690449b18480f809fb3243f800d97717a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c#L574-L769
• https://github.com/robotology/icub-firmware/blob/e9e5167690449b18480f809fb3243f800d97717a/emBODY/eBcode/arch-arm/embobj/plus/mc/Pid.c#L120-L192

[PeterBowman]

This was done as part of https://github.com/roboticslab-uc3m/yarp-devices/issues/262. I have successfully tested this on ID5 with yarpmotorgui, which also provides a nice interface for checking and tweaking impedance parameters (PID button):

[PeterBowman]

Oh, by the way. This implementation somewhat performs impedance control in the motor space. Joint space probably requires friction compensation (https://github.com/roboticslab-uc3m/yarp-devices/issues/261).