Changing interaction parameters dynamically during trajectory execution

[Danfei]

Hi,

we are trying to make our Sawyer to execute Cartesian trajectories with dynamically changing interaction parameters (impedance), e.g. the waypoints are defined as 6D poses with associated impedance parameters. As first attempt we tried to use the motion interface to execute the Cartesian trajectory. However, in this interface the interaction parameters are constant (a property of the trajectory) and cannot be defined per waypoint. We tried to "hack" it by publishing our own InteractionControlCommand's during the trajectory execution but the node "motion_controller" on the Sawyer computer periodically publishes the parameters defined in the trajectory and the messages clash.

Did someone succeed on implementing a similar functionality?

--Roberto

[Emmanuel]

Hi Roberto,

 

I actually attempted to implement a very similar algorithm to the one described in your post, and discovered that there is a persistent confliction between the trajectory tracking (Inverse Dynamics) controller, and the interaction (impedance) controller. The latter therefore inhibits the seamless operation of the former, and vice versa, since the execution of the trajectory server 'ensures' that the low-level control is constantly dictated by the tracking controller. It can be observed however that the interaction controller is intermittently activated, thereby inducing unstable oscillations into the system.

It appears that there is currently no means of modulating the Inverse Dynamics controller's PD gains in real time, hence why my pertinent query remains unanswered to this day:

 

 

Kind regards,

Emmanuel

[Danfei]

Hi Emmanuel,

as a workaround I got close to the desired functionality by replicating part of the trajectory interface. Basically I compute a joint space trajectory to follow a set of Cartesian way points and interpolate the impedance parameters between them. Then, I command the joint configurations while synchronously publishing the dynamically changing impedance values in /robot/limb/right/interaction_control_command . This is right now "open loop" in a sense that I do not check for the execution of the trajectory and the current state on the plan, but just periodically send a new joint pose.

Emmanuel, when you say trajectory tracking, do you mean TRAJECTORY_MODE in JointCommand messages or using the joint trajectory action server? I didn't try specifying a full trajectory (joint pose, velocity and acceleration) but it would be nice to be able to do it, too.

It would be of course much easier if the motion_controller on the Sawyer machine would do the interpolation of impedance parameters (passed as parameters of the waypoints), but that would involve changing the "firmware" on Rethink's side...

Regards,

Roberto

[Emmanuel]

Hi Roberto,

 

Thank you for the insightful reply.

 

I have been exploring the option of utilising the IK functions directly to compute the joint angles for a desired Cartesian trajectory, since last week, although my algorithm currently modulates the impedances based on the 'wrench feedback'. As per your description, the latter does seem to function aptly, even though Rethink's firmware-implemented controller may not provide the option of regulating the impedance values in real time.

I should highlight that my previous experiments revolved around the use of the joint trajectory action server, as you mentioned, which is on occasion undesirably sensitive to the predefined position error margins.

It seems as though we may have to conform with the IK + impedance modulation solution for the time being.

 

Kind regards,

Emmanuel