Adding equality constraints between joints

[sjauhri]

Installation: TSID version 1.6.3 from conda

Hello team,
I am having trouble adding an equality constraint between joints in the tsid formulation.
For context, I am trying to add a differential-drive constraint for my robot (and my code is similar to the examples: github.com/stack-of-tasks/tsid/blob/master/exercizes/ex_1_ur5.py)

I am able to create the equality constraint using the tsid.ConstraintEquality class in the following manner:

A = np.zeros((model.nv,1))
b = np.zeros((model.nv,1))
diffdriveConstraint = tsid.ConstraintEquality("diff-drive", A, b)
diffdriveConstraintLevel = tsid.ConstraintLevel()
diffdriveConstraintLevel.append(1.0, diffdriveConstraint)

However, this ConstraintLevel can't be combined with the rest of the tasks in the tsid formulation i.e. tsid.formulation.computeProblemData()... The following code will throw an error:

HQPData = tsid.HQPData()
HQPData.append(diffdriveConstraintLevel)
HQPData.append(tsid.formulation.computeProblemData(t, q[:,i], v[:,i]))

Error: Python argument types in HQPData.append(HQPData, HQPData)
did not match C++ signature: append(tsid::python::HQPDatas {lvalue}, tsid::python::ConstraintLevels constraintLevel)

In essence, I need a way to either:
Option 1. Somehow insert the created ConstraintLevel at any appropriate place within the already generated HQPData from tsid.formulation.computeProblemData(t, q[:,i], v[:,I])
OR
Option 2: Somehow create a task in the tsid formulation that can handle this kind of diff-drive constraint (equality constraint between joints)

How can I go about this?

[jcarpent]

Could you share a complete but minimal reproducible test?

[sjauhri]

Here is the file for a minimal example:
(ex_1_ur5.py) https://drive.google.com/drive/folders/14QwTRxESBkLBdqrMRpwobuxkHMGEiCxf?usp=sharing
Copy ex_1_ur5.py to the tsid/exercizes directory in the tsid repo and run: python ex_1_ur5.py
I've also included a conda environment.yml if needed (the env mainly just contains tsid=1.6.3 and matplotlib)

I have added the main code under consideration at line 68: equality_contraint = True and added comments related to the issue

[andreadelprete]

How can I go about this?

You can achieve your goal by using a JointPostureTask, something like:

        postureTask = tsid.TaskJointPosture("task-posture", robot)
        postureTask.setKp(conf.kp_posture * conf.gain_vector)
        postureTask.setKd(2.0 * np.sqrt(conf.kp_posture * conf.gain_vector))
        postureTask.setMask(conf.masks_posture)
        formulation.addMotionTask(postureTask, conf.w_posture, 0, 0.0)

This code was taken from this example, with the only modification being the hierarchical level at which I added the motion task, which I've changed from 1 (cost function) to 0 (constraints). Make sure to use the mask to select the joint(s) that you want to constrain.

[sjauhri]

Thanks @andreadelprete.
If I understand this correctly, using the postureTask would help in setting my required joints to a particular reference value but in my case, I want to add a constraint between the joints. For example, if I want to always set joint0=-joint1 but I don't want to explicitly set them to any particular value, how do I do this?

In the code I shared above, I tried doing this with constraint matrices A & b (line 71 in ex_1_ur5.py)

[andreadelprete]

Ok, now I see what you need. For this you need to create a new task, similar to the joint posture task, but that allows you to constrain the joint positions relative to each other. The task will need to be written in C++ and then binded in Python, as all the other tasks.

You cannot work directly with the Constraint objects in python as you were trying to do (or at least I am not aware of anyone who tried and the library was not designed for this use case).

[sjauhri]

Thanks for the answer @andreadelprete.
I mainly needed this to set differential-drive constraints for my robot's base wheels. For now, I can manage without adding this constraint but just hacking my robot's urdf instead.

Since the underlying QP solver definitely accepts any constraint of the form Ax = b, it would be very convenient and flexible for a user to add such a constraint to the generated HQPData formulation, at least at the constraint level (level 0).

Maybe you could consider adding this as an enhancement?

[andreadelprete]

I can look into it in January. Maybe it's easy.

[sjauhri]

Hi @andreadelprete,
Did you get a chance to look into this?

[andreadelprete]

Not yet