ForceTask with moving object

[paulinejmaurice]

Hi!
I would like to use a Force Task in the case where the external force comes from a contact with an object that is attached to the robot (I'm doing torque control). For instance, if a robot is supposed to carry a load, I'd like to include this information so the controller is aware of it (rather than considering the load/external force as a perturbation and increasing the gains of motion tasks to maintain a correct tracking).
In such a case, i) the "contact point" (force application point) is not fixed in the world frame, but moving with the robot, and ii) the contact is bi-lateral, i.e. cannot be broken because the object is considered attached to the robot (unlike foot/ground contact for instance).
I looked into the ContactForceEquality Task, but as far as I understand, it is not completely suitable in such a case. From what i understood, the ContactForceEquality task necessarily involves a fixed unilateral contact (i.e. adds to the QP constraints that correspond to a zero velocity of the contact point within the world frame + non-slipping condition constraints, which do not apply in my case).
So my questions are:

• Am I right about the fact that the ContactForceEquality is not suitable for a contact with an object attached to the robot?
• Is there currently another task that would correspond to what I am looking for?
• If not and I need to implement a dedicated task, would you recommend that it inherits from the task-contact-force class, or do I need a totally different task (because I don't think I want to create contact constraints associated to the task in my case).
  Thanks

[andreadelprete]

Hi @paulinejmaurice ,

yes, I think you need to implement a new class to achieve your goal. You shouldn't use a Contact class, because contacts are supposed to create new contact force variables in the QP, which is not needed in your case, because you are measuring the contact force. In this case, this corresponds to a variation of the bias forces in the robot dynamics, so it could be simply addressed in the robot wrapper. However, adding this to the robot wrapper doesn't seem clean to me, so it would be better to extend the InverseDynamicsFormulation class instead. Maybe we could add a method to account for measured contact forces in the dynamics. These measured contact forces should be associated to a frame, a dimension (3 for pure forces, 6 for wrenches), and at each iteration the user should take care of updating the measured force value in this object (similarly to what is done for the reference task trajectories). Does this sound reasonable to you?

[paulinejmaurice]

Hi @andreadelprete,
Thanks for your quick reply.
What your are suggesting is indeed aligned with what I had in mind, since as you say, the crucial point is to add, at each time step, the measured contact forces to the dynamics.

[andreadelprete]

Ok. In this case my suggestion is to proceed as follows:

• Create a class MeasuredForceBase to represent this new concept of measured contact forces. This base class should be similar to a TaskContactForce, in the sense that it should take as input just a name and a reference to a robot wrapper. In addition, it should declare an abstract method computeJointTorques to compute the joint torques generated by the measured force.
• Create a child class of MeasuredForceBase, with an appropriate name depending on the size of the contact force you want to consider. The constructor of this class should take as input also the name of the frame associated to the contact, possibly a mask (if you need it), possibly a flag to specify whether quantities are expressed in local or local-world-aligned frame. It should also implement a method to allow the user to set the measured contact force, and it should of course implement the method computeJointTorques, that maps this contact forces to the joints.
• In InverseDynamicsFormulationBase add a new method addMeasuredForce that takes as input (a reference to) an object of class MeasuredForceBase, which is then stored in a vector of shared pointers in InverseDynamicsFormulationAccForce
• In this line of InverseDynamicsFormulationAccForce, modify the computation of the bias forces h to account for measured contact forces, by looping over the list of measured contact forces and adding their contribution to h.

[paulinejmaurice]

Ok, thanks for detailing the steps. Will do asap and keep you updated.

[paulinejmaurice]

Hi @andreadelprete
I am wondering if the measured contact forces also need to be added in the method "decodeSolution", in class InverseDynamicsFormulationAccForce? Thanks.

[andreadelprete]

Hi @andreadelprete I am wondering if the measured contact forces also need to be added in the method "decodeSolution", in class InverseDynamicsFormulationAccForce? Thanks.

Good question. I don't think it should be added there because the measure contact force is not found by the solver, but it is set by the user.

[paulinejmaurice]

I looked at the code again, and I believe the measure contact force should be added to h_a in the method decodeSolution.
The reason I believe so, is that in the method computeProblemData, I am adding the measured forces to h_a (bias force) (l.341). But h_a is only used in the remaining of this method to compute the gradient and hessian of actuation tasks, because joint torques are not in the variables of the problem. So if I don't define any actuation task, then these measured forces are ignored. Yet, I still want the controller to account for this measured forces even if I only have, let's say, motion tasks (such that the resulting motion is "correct" without having to artificially increase the gains a lot, which would probably give a similar result, since it would increase the resistance to unknown perturbations).
If I add the measured force also in the decodeSolution method, then the joint torques resulting from the optimization will take into account these measured force, whether actuation tasks are defined or not. And this is, I believe, the behavior I want if I am doing torque control (which I am).
I also checked experimentally: if I add a measured force without adding it to the decodeSolution method, then the joint torques computed with the QP are not affected at all by this measured force, which is weird. However, they are if the measured force is added in the decodeSoltuion method.

[andreadelprete]

I agree with you. My previous answer came from a misunderstanding of your previous question.

[Groulaxx]

Hello @andreadelprete , Now that the contribution we wanted to add (working with @paulinejmaurice )is working in our local branch, How do I need to process for the "Pull Request" ?
Maybe I need to merge using upstream/devel into my branch and then I send the Pull request ?

[andreadelprete]

Yes exactly

[olivier-stasse]

Fixed by PR #196
Feel free to reopen it if needed.