Repository: stack-of-tasks/tsid (language: python)

Issue:
Improve computation time of HQP solver
Currently the only QP solver implemented is eiquadprog, which is already rather efficient, but there's room for improvement. In particular, the current version of the solver allocates dynamic memory at each call to the solver, so it should be easy to decrease computation time by pre-allocating all the necessary memory in an initialization phase.


Discussion / hints:
In the last commit 8788221c89d07ddbcc118f4f322c69bb4b284890 I added two improved versions of eiquadprog, neither of which allocates dynamic memory during resolution (verified using Eigen function ```set_is_malloc_allowed```). The only difference between them is that RtEiquadprog uses matrices with size known at compile time, whereas EiquadprogFast does not. In both solvers I optimized a bit the code using some Eigen tricks such as ```noalias```, or ```solveInPlace```.  Finally, both new solvers have also more documentation and provide more information to the interested user, such as number of iterations, number of active inequalities, maximum number of iterations and proper status flag in case the problem could not be solved.
Thiese are the results I got solving random QP on HRP-2's computer (but in a non-real-time thread). More or less we gained 15% of the computation time. The real-time solver is only slightly faster than the "fast" solver.  The size of the problems has been chosen to match the size of the QP we typically have to solve in our inverse-dynamics balance controller.

> 1000 tests with 60 variables, 18 equalities, 40 inequalities
> *** PROFILING RESULTS [ms]                       (min - avg - max - nSamples) ***
> Eiquadprog ...................                                        0.651    0.704    0.870     1000
> Eiquadprog Fast ............                                        0.563    0.605    0.810     1000
> Eiquadprog Real Time ....                                       0.543    0.592    0.712      1000
> 
> *** STATISTICS (min - avg - max - nSamples) ***
> active inequalities       ....                                  16.0    19.8    26.0      1000
> solver iterations  ........                                         17.0    21.5    29.0      1000
FYI: I also tried to warm start the algorithm (see macro ```USE_WARM_START``` in ```eiquadprog_fast.h```) but I didn't manage to make it work properly (it only worked in the trivial case where the active set didn't change). Then I decided to stop working on it, but maybe it's just a matter of fixing a simple bug to make it work, in case someone is interested in finishing the work (the code should be quite self-explaining now because I added plenty of comments).
I've just discovered a new [open-source QP solver](https://github.com/asherikov/qpmad), which is supposed to be faster than eiquadprog, so we may give it a try by integrating it inside TSID. Anybody feels like doing it?
Since this issue has been inactive for a while now I think we can close it.

Fix patch:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0c45d6c2..11a6356d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -147,7 +147,8 @@ set(${PROJECT_NAME}_TASKS_HEADERS
     include/tsid/tasks/task-joint-posture.hpp
     include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
     include/tsid/tasks/task-capture-point-inequality.hpp
-    include/tsid/tasks/task-angular-momentum-equality.hpp)
+    include/tsid/tasks/task-angular-momentum-equality.hpp
+    include/tsid/tasks/task-two-frames-equality.hpp)
 
 set(${PROJECT_NAME}_CONTACTS_HEADERS
     include/tsid/contacts/fwd.hpp
@@ -156,7 +157,8 @@ set(${PROJECT_NAME}_CONTACTS_HEADERS
     include/tsid/contacts/contact-point.hpp
     include/tsid/contacts/measured-force-base.hpp
     include/tsid/contacts/measured-3Dforce.hpp
-    include/tsid/contacts/measured-6Dwrench.hpp)
+    include/tsid/contacts/measured-6Dwrench.hpp
+    include/tsid/contacts/contact-two-frame-positions.hpp)
 
 set(${PROJECT_NAME}_TRAJECTORIES_HEADERS
     include/tsid/trajectories/fwd.hpp
@@ -264,12 +266,17 @@ set(${PROJECT_NAME}_TASKS_SOURCES
     src/tasks/task-capture-point-inequality.cpp
     src/tasks/task-motion.cpp
     src/tasks/task-se3-equality.cpp
-    src/tasks/task-angular-momentum-equality.cpp)
+    src/tasks/task-angular-momentum-equality.cpp
+    src/tasks/task-two-frames-equality.cpp)
 
 set(${PROJECT_NAME}_CONTACTS_SOURCES
-    src/contacts/contact-base.cpp src/contacts/contact-6d.cpp
-    src/contacts/contact-point.cpp src/contacts/measured-force-base.cpp
-    src/contacts/measured-3Dforce.cpp src/contacts/measured-6Dwrench.cpp)
+    src/contacts/contact-base.cpp
+    src/contacts/contact-6d.cpp
+    src/contacts/contact-point.cpp
+    src/contacts/measured-force-base.cpp
+    src/contacts/measured-3Dforce.cpp
+    src/contacts/measured-6Dwrench.cpp
+    src/contacts/contact-two-frame-positions.cpp)
 
 set(${PROJECT_NAME}_TRAJECTORIES_SOURCES
     src/trajectories/trajectory-se3.cpp
diff --git a/bindings/python/contacts/contact-two-frame-positions.cpp b/bindings/python/contacts/contact-two-frame-positions.cpp
new file mode 100644
index 00000000..5a148ccb
--- /dev/null
+++ b/bindings/python/contacts/contact-two-frame-positions.cpp
@@ -0,0 +1,29 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include "tsid/bindings/python/contacts/contact-two-frame-positions.hpp"
+#include "tsid/bindings/python/contacts/expose-contact.hpp"
+
+namespace tsid {
+namespace python {
+void exposeContactTwoFramePositions() {
+  ContactTwoFramePositionsPythonVisitor<
+      tsid::contacts::ContactTwoFramePositions>::
+      expose("ContactTwoFramePositions");
+}
+}  // namespace python
+}  // namespace tsid
diff --git a/bindings/python/tasks/task-two-frames-equality.cpp b/bindings/python/tasks/task-two-frames-equality.cpp
new file mode 100644
index 00000000..08c02227
--- /dev/null
+++ b/bindings/python/tasks/task-two-frames-equality.cpp
@@ -0,0 +1,28 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include "tsid/bindings/python/tasks/task-two-frames-equality.hpp"
+#include "tsid/bindings/python/tasks/expose-tasks.hpp"
+
+namespace tsid {
+namespace python {
+void exposeTaskTwoFramesEquality() {
+  TaskTwoFramesEqualityPythonVisitor<
+      tsid::tasks::TaskTwoFramesEquality>::expose("TaskTwoFramesEquality");
+}
+}  // namespace python
+}  // namespace tsid
diff --git a/demo/demo_quadruped.py b/demo/demo_quadruped.py
index 411eb0ad..b708671f 100644
--- a/demo/demo_quadruped.py
+++ b/demo/demo_quadruped.py
@@ -6,11 +6,10 @@
 import gepetto.corbaserver
 import numpy as np
 import pinocchio as pin
+import tsid
 from numpy import nan
 from numpy.linalg import norm as norm
 
-import tsid
-
 sys.path += [os.getcwd() + "/../exercizes"]
 import matplotlib.pyplot as plt
 import plot_utils as plut
diff --git a/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
new file mode 100644
index 00000000..e48918d2
--- /dev/null
+++ b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
@@ -0,0 +1,196 @@
+"""
+Simple demo of usage of ContactTwoFramePositions in TSID
+Make the Talos gripper model works with closed kinematic chains
+(c) MIPT
+"""
+import os
+import time
+
+import numpy as np
+import pinocchio as pin
+import tsid
+from numpy import nan
+from numpy.linalg import norm as norm
+
+np.set_printoptions(precision=3, linewidth=200, suppress=True)
+
+LINE_WIDTH = 60
+print("".center(LINE_WIDTH, "#"))
+print(
+    " Demo of TSID with Closed Kinematic Chains via ContactTwoFramePositions".center(
+        LINE_WIDTH, "#"
+    )
+)
+print("".center(LINE_WIDTH, "#"), "\n")
+
+w_ee = 1.0  # weight of end effector task
+w_posture = 1e-3  # weight of joint posture task
+
+kp_ee = 10.0  # proportional gain of end effector task
+kp_posture = 10.0  # proportional gain of joint posture task
+
+dt = 0.001  # controller time step
+PRINT_N = 500  # print every PRINT_N time steps
+DISPLAY_N = 25  # update robot configuration in viwewer every DISPLAY_N time steps
+N_SIMULATION = 6000  # number of time steps simulated
+
+# Loading Talos gripper model modified with extra links to mark the position of contact creation
+# Talos gripepr model (c) 2016, PAL Robotics, S.L.
+# Please use https://github.com/egordv/tsid_demo_closed_kinematic_chain repo for the model files
+
+filename = str(os.path.dirname(os.path.abspath(__file__)))
+path = filename + "../../tsid_demo_closed_kinematic_chain/models/talos_gripper"
+urdf = path + "../../tsid_demo_closed_kinematic_chain/urdf/talos_gripper_half.urdf"
+vector = pin.StdVec_StdString()
+vector.extend(item for item in path)
+
+robot = tsid.RobotWrapper(urdf, vector, False)  # Load with fixed base
+
+# for viewer
+robot_display = pin.RobotWrapper.BuildFromURDF(urdf, [path])
+robot_display.initViewer(loadModel=True)
+
+model = robot.model()
+q = np.zeros(robot.nq)
+v = np.zeros(robot.nv)
+
+viz = pin.visualize.MeshcatVisualizer(
+    robot_display.model,
+    robot_display.collision_model,
+    robot_display.visual_model,
+)
+viz.initViewer(loadModel=True, open=True)
+
+t = 0.0  # time
+invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
+invdyn.computeProblemData(t, q, v)
+data = invdyn.data()
+
+
+fingertip_name = "gripper_left_fingertip_3_link"
+H_fingertip_ref = robot.framePosition(invdyn.data(), model.getFrameId(fingertip_name))
+
+fingertipPositionTask = tsid.TaskSE3Equality(
+    "task-fingertip-position", robot, fingertip_name
+)
+fingertipPositionTask.useLocalFrame(False)
+fingertipPositionTask.setKp(kp_ee * np.ones(6))
+fingertipPositionTask.setKd(2.0 * np.sqrt(kp_ee) * np.ones(6))
+trajFingertipPosition = tsid.TrajectorySE3Constant(
+    "traj-fingertip-position", H_fingertip_ref
+)
+sampleFingertipPosition = trajFingertipPosition.computeNext()
+fingertipPositionTask.setReference(sampleFingertipPosition)
+invdyn.addMotionTask(fingertipPositionTask, w_ee, 1, 0.0)
+
+postureTask = tsid.TaskJointPosture("task-posture", robot)
+postureTask.setKp(kp_posture * np.ones(robot.nv))
+postureTask.setKd(2.0 * np.sqrt(kp_posture) * np.ones(robot.nv))
+invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
+
+
+# Creating a closed kinematic chain in TSID formulation by creating a contact between two frames, for which there are special links in URDF
+ContactTwoFramePositionsFingertipBottomAxis = tsid.ContactTwoFramePositions(
+    "contact-two-frame-positions-fingertip-bottom-axis",
+    robot,
+    "gripper_left_motor_single_link_ckc_axis",
+    "gripper_left_fingertip_3_link_ckc_axis",
+    -1000,
+    1000,
+)
+twoFramesContact_Kp = 300
+ContactTwoFramePositionsFingertipBottomAxis.setKp(twoFramesContact_Kp * np.ones(3))
+ContactTwoFramePositionsFingertipBottomAxis.setKd(
+    2.0 * np.sqrt(twoFramesContact_Kp) * np.ones(3)
+)
+
+twoFramesContact_w_forceRef = 1e-5
+invdyn.addRigidContact(
+    ContactTwoFramePositionsFingertipBottomAxis, twoFramesContact_w_forceRef, 1.0, 1
+)
+
+# Setting actuation to zero for passive joints in kinematic chain via TaskActuationBounds
+tau_max = model.effortLimit[-robot.na :]
+tau_max[
+    0
+] = 0.0  # setting gripper_left_inner_single_joint to passive contstrainig it's actuation bounds to zero
+tau_max[
+    1
+] = 0.0  # setting gripper_left_fingertip_3_joint to passive contstrainig it's actuation bounds to zero
+tau_min = -tau_max
+actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
+actuationBoundsTask.setBounds(tau_min, tau_max)
+invdyn.addActuationTask(actuationBoundsTask, 1.0, 0, 0.0)
+
+q_ref = q
+trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
+
+print(
+    "Create QP solver with ",
+    invdyn.nVar,
+    " variables, ",
+    invdyn.nEq,
+    " equality and ",
+    invdyn.nIn,
+    " inequality constraints",
+)
+solver = tsid.SolverHQuadProgFast("qp solver")
+solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
+
+offset = sampleFingertipPosition.pos()
+offset[:3] += np.array([0, -0.04, 0])
+amp = np.array([0.0, 0.04, 0.0])
+two_pi_f = 2 * np.pi * np.array([0.0, 0.5, 0.0])
+two_pi_f_amp = np.multiply(two_pi_f, amp)
+two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
+
+pEE = offset.copy()
+vEE = np.zeros(6)
+aEE = np.zeros(6)
+
+i = 0
+while True:
+    time_start = time.time()
+
+    # Setting gripper finger task target to sine motion
+    pEE[:3] = offset[:3] + amp * np.sin(two_pi_f * t)
+    vEE[:3] = two_pi_f_amp * np.cos(two_pi_f * t)
+    aEE[:3] = -two_pi_f_squared_amp * np.sin(two_pi_f * t)
+    sampleFingertipPosition.value(pEE)
+    sampleFingertipPosition.derivative(vEE)
+    sampleFingertipPosition.second_derivative(aEE)
+
+    fingertipPositionTask.setReference(sampleFingertipPosition)
+
+    samplePosture = trajPosture.computeNext()
+    postureTask.setReference(samplePosture)
+
+    # Computing HQP
+    HQPData = invdyn.computeProblemData(t, q, v)
+    if i == 0:
+        HQPData.print_all()
+
+    sol = solver.solve(HQPData)
+    if sol.status != 0:
+        print("[%d] QP problem could not be solved! Error code:" % (i), sol.status)
+        break
+
+    tau = invdyn.getActuatorForces(sol)
+    dv = invdyn.getAccelerations(sol)
+
+    if i % PRINT_N == 0:
+        print("Time %.3f" % (t))
+
+    v_mean = v + 0.5 * dt * dv
+    v += dt * dv
+    q = pin.integrate(robot.model(), q, dt * v_mean)
+    t += dt
+
+    if i % DISPLAY_N == 0:
+        viz.display(q)
+
+    time_spent = time.time() - time_start
+    if time_spent < dt:
+        time.sleep(dt - time_spent)
+
+    i = i + 1
diff --git a/exercizes/ex_0_ur5_joint_space_control.py b/exercizes/ex_0_ur5_joint_space_control.py
index 290e60f7..bee28a8e 100644
--- a/exercizes/ex_0_ur5_joint_space_control.py
+++ b/exercizes/ex_0_ur5_joint_space_control.py
@@ -7,12 +7,11 @@
 import numpy as np
 import pinocchio as pin
 import plot_utils as plut
+import tsid
 import ur5_conf as conf
 from numpy import nan
 from numpy.linalg import norm as norm
 
-import tsid
-
 print("".center(conf.LINE_WIDTH, "#"))
 print(" Joint Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, "#"))
 print("".center(conf.LINE_WIDTH, "#"), "\n")
diff --git a/exercizes/ex_4_walking.py b/exercizes/ex_4_walking.py
index fceec37c..18d0120c 100644
--- a/exercizes/ex_4_walking.py
+++ b/exercizes/ex_4_walking.py
@@ -4,12 +4,11 @@
 import matplotlib.pyplot as plt
 import numpy as np
 import plot_utils as plut
+import tsid
 from numpy import nan
 from numpy.linalg import norm as norm
 from tsid_biped import TsidBiped
 
-import tsid
-
 print("".center(conf.LINE_WIDTH, "#"))
 print(" Test Walking ".center(conf.LINE_WIDTH, "#"))
 print("".center(conf.LINE_WIDTH, "#"), "\n")
diff --git a/exercizes/notebooks/Installation process.ipynb b/exercizes/notebooks/Installation process.ipynb
index de8bd7cc..0923bcf9 100644
--- a/exercizes/notebooks/Installation process.ipynb
+++ b/exercizes/notebooks/Installation process.ipynb
@@ -1,5 +1,5 @@
 {
- "cells": [
+  "cells": [
   {
    "cell_type": "markdown",
    "metadata": {},
diff --git a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
index 1e1642e7..04eee69d 100644
--- a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
+++ b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
@@ -1,5 +1,5 @@
 {
- "cells": [
+  "cells": [
   {
    "cell_type": "markdown",
    "metadata": {},
diff --git a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
index a5e1f8c4..1d707a61 100644
--- a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
+++ b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
@@ -1,5 +1,5 @@
 {
- "cells": [
+  "cells": [
   {
    "cell_type": "markdown",
    "metadata": {},
diff --git a/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb b/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
index f6db703c..c63fad6d 100644
--- a/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
+++ b/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
@@ -1,5 +1,5 @@
 {
- "cells": [
+  "cells": [
   {
    "cell_type": "code",
    "execution_count": null,
diff --git a/exercizes/tsid_biped.py b/exercizes/tsid_biped.py
index d63d25ae..58a229c6 100644
--- a/exercizes/tsid_biped.py
+++ b/exercizes/tsid_biped.py
@@ -4,7 +4,6 @@
 
 import numpy as np
 import pinocchio as pin
-
 import tsid
 
 
diff --git a/exercizes/tsid_manipulator.py b/exercizes/tsid_manipulator.py
index 93a7bc83..c8001270 100644
--- a/exercizes/tsid_manipulator.py
+++ b/exercizes/tsid_manipulator.py
@@ -6,7 +6,6 @@
 import numpy as np
 import numpy.matlib as matlib
 import pinocchio as se3
-
 import tsid
 
 
diff --git a/include/tsid/bindings/python/contacts/contact-two-frame-positions.hpp b/include/tsid/bindings/python/contacts/contact-two-frame-positions.hpp
new file mode 100644
index 00000000..f7de1315
--- /dev/null
+++ b/include/tsid/bindings/python/contacts/contact-two-frame-positions.hpp
@@ -0,0 +1,204 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __tsid_python_contact_two_frame_positions_hpp__
+#define __tsid_python_contact_two_frame_positions_hpp__
+
+#include "tsid/bindings/python/fwd.hpp"
+
+#include "tsid/contacts/contact-two-frame-positions.hpp"
+#include "tsid/robots/robot-wrapper.hpp"
+#include "tsid/math/constraint-inequality.hpp"
+#include "tsid/math/constraint-equality.hpp"
+#include "tsid/math/constraint-base.hpp"
+
+namespace tsid {
+namespace python {
+namespace bp = boost::python;
+
+template <typename ContactTwoFramePositions>
+struct ContactTwoFramePositionsPythonVisitor
+    : public boost::python::def_visitor<
+          ContactTwoFramePositionsPythonVisitor<ContactTwoFramePositions> > {
+  template <class PyClass>
+
+  void visit(PyClass& cl) const {
+    cl
+        .def(bp::init<std::string, robots::RobotWrapper&, std::string,
+                      std::string, double, double>(
+            (bp::arg("name"), bp::arg("robot"), bp::arg("framename1"),
+             bp::arg("framename2"), bp::arg("minForce"), bp::arg("maxForce")),
+            "Default Constructor"))
+        .add_property("n_motion", &ContactTwoFramePositions::n_motion,
+                      "return number of motion")
+        .add_property("n_force", &ContactTwoFramePositions::n_force,
+                      "return number of force")
+        .add_property("name", &ContactTwoFramePositionsPythonVisitor::name,
+                      "return name")
+        .def("computeMotionTask",
+             &ContactTwoFramePositionsPythonVisitor::computeMotionTask,
+             bp::args("t", "q", "v", "data"))
+        .def("computeForceTask",
+             &ContactTwoFramePositionsPythonVisitor::computeForceTask,
+             bp::args("t", "q", "v", "data"))
+        .def("computeForceRegularizationTask",
+             &ContactTwoFramePositionsPythonVisitor::
+                 computeForceRegularizationTask,
+             bp::args("t", "q", "v", "data"))
+
+        .add_property(
+            "getForceGeneratorMatrix",
+            bp::make_function(
+                &ContactTwoFramePositionsPythonVisitor::getForceGeneratorMatrix,
+                bp::return_value_policy<bp::copy_const_reference>()))
+
+        .def("getNormalForce",
+             &ContactTwoFramePositionsPythonVisitor::getNormalForce,
+             bp::arg("vec"))
+        .add_property("getMinNormalForce",
+                      &ContactTwoFramePositions::getMinNormalForce)
+        .add_property("getMaxNormalForce",
+                      &ContactTwoFramePositions::getMaxNormalForce)
+
+        .add_property("Kp",
+                      bp::make_function(
+                          &ContactTwoFramePositionsPythonVisitor::Kp,
+                          bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("Kd",
+                      bp::make_function(
+                          &ContactTwoFramePositionsPythonVisitor::Kd,
+                          bp::return_value_policy<bp::copy_const_reference>()))
+        .def("setKp", &ContactTwoFramePositionsPythonVisitor::setKp,
+             bp::arg("Kp"))
+        .def("setKd", &ContactTwoFramePositionsPythonVisitor::setKd,
+             bp::arg("Kd"))
+        .def("setContactNormal",
+             &ContactTwoFramePositionsPythonVisitor::setContactNormal,
+             bp::args("vec"))
+        .def("setFrictionCoefficient",
+             &ContactTwoFramePositionsPythonVisitor::setFrictionCoefficient,
+             bp::args("friction_coeff"))
+        .def("setMinNormalForce",
+             &ContactTwoFramePositionsPythonVisitor::setMinNormalForce,
+             bp::args("min_force"))
+        .def("setMaxNormalForce",
+             &ContactTwoFramePositionsPythonVisitor::setMaxNormalForce,
+             bp::args("max_force"))
+        .def("setForceReference",
+             &ContactTwoFramePositionsPythonVisitor::setForceReference,
+             bp::args("f_vec"))
+        .def("setRegularizationTaskWeightVector",
+             &ContactTwoFramePositionsPythonVisitor::
+                 setRegularizationTaskWeightVector,
+             bp::args("w_vec"));
+  }
+  static std::string name(ContactTwoFramePositions& self) {
+    std::string name = self.name();
+    return name;
+  }
+
+  static math::ConstraintEquality computeMotionTask(
+      ContactTwoFramePositions& self, const double t, const Eigen::VectorXd& q,
+      const Eigen::VectorXd& v, pinocchio::Data& data) {
+    self.computeMotionTask(t, q, v, data);
+    math::ConstraintEquality cons(self.getMotionConstraint().name(),
+                                  self.getMotionConstraint().matrix(),
+                                  self.getMotionConstraint().vector());
+    return cons;
+  }
+  static math::ConstraintInequality computeForceTask(
+      ContactTwoFramePositions& self, const double t, const Eigen::VectorXd& q,
+      const Eigen::VectorXd& v, const pinocchio::Data& data) {
+    self.computeForceTask(t, q, v, data);
+    math::ConstraintInequality cons(self.getForceConstraint().name(),
+                                    self.getForceConstraint().matrix(),
+                                    self.getForceConstraint().lowerBound(),
+                                    self.getForceConstraint().upperBound());
+    return cons;
+  }
+  static math::ConstraintEquality computeForceRegularizationTask(
+      ContactTwoFramePositions& self, const double t, const Eigen::VectorXd& q,
+      const Eigen::VectorXd& v, const pinocchio::Data& data) {
+    self.computeForceRegularizationTask(t, q, v, data);
+    math::ConstraintEquality cons(self.getForceRegularizationTask().name(),
+                                  self.getForceRegularizationTask().matrix(),
+                                  self.getForceRegularizationTask().vector());
+    return cons;
+  }
+
+  static const Eigen::MatrixXd& getForceGeneratorMatrix(
+      ContactTwoFramePositions& self) {
+    return self.getForceGeneratorMatrix();
+  }
+  static const Eigen::VectorXd& Kp(ContactTwoFramePositions& self) {
+    return self.Kp();
+  }
+  static const Eigen::VectorXd& Kd(ContactTwoFramePositions& self) {
+    return self.Kd();
+  }
+  static void setKp(ContactTwoFramePositions& self,
+                    const ::Eigen::VectorXd Kp) {
+    return self.Kp(Kp);
+  }
+  static void setKd(ContactTwoFramePositions& self,
+                    const ::Eigen::VectorXd Kd) {
+    return self.Kd(Kd);
+  }
+  static bool setContactTwoFramePositionss(
+      ContactTwoFramePositions& self,
+      const ::Eigen::MatrixXd ContactTwoFramePositionss) {
+    return self.setContactTwoFramePositionss(ContactTwoFramePositionss);
+  }
+  static bool setContactNormal(ContactTwoFramePositions& self,
+                               const ::Eigen::VectorXd contactNormal) {
+    return self.setContactNormal(contactNormal);
+  }
+  static bool setFrictionCoefficient(ContactTwoFramePositions& self,
+                                     const double frictionCoefficient) {
+    return self.setFrictionCoefficient(frictionCoefficient);
+  }
+  static bool setMinNormalForce(ContactTwoFramePositions& self,
+                                const double minNormalForce) {
+    return self.setMinNormalForce(minNormalForce);
+  }
+  static bool setMaxNormalForce(ContactTwoFramePositions& self,
+                                const double maxNormalForce) {
+    return self.setMaxNormalForce(maxNormalForce);
+  }
+  static void setForceReference(ContactTwoFramePositions& self,
+                                const ::Eigen::VectorXd f_ref) {
+    self.setForceReference(f_ref);
+  }
+  static void setRegularizationTaskWeightVector(ContactTwoFramePositions& self,
+                                                const ::Eigen::VectorXd w) {
+    self.setRegularizationTaskWeightVector(w);
+  }
+  static double getNormalForce(ContactTwoFramePositions& self,
+                               Eigen::VectorXd f) {
+    return self.getNormalForce(f);
+  }
+  static void expose(const std::string& class_name) {
+    std::string doc = "ContactTwoFramePositions info.";
+    bp::class_<ContactTwoFramePositions>(class_name.c_str(), doc.c_str(),
+                                         bp::no_init)
+        .def(ContactTwoFramePositionsPythonVisitor<ContactTwoFramePositions>());
+  }
+};
+}  // namespace python
+}  // namespace tsid
+
+#endif  // ifndef __tsid_python_contact_two_frame_positions_hpp__
diff --git a/include/tsid/bindings/python/contacts/contact-two-frames.hpp b/include/tsid/bindings/python/contacts/contact-two-frames.hpp
new file mode 100644
index 00000000..7889efb7
--- /dev/null
+++ b/include/tsid/bindings/python/contacts/contact-two-frames.hpp
@@ -0,0 +1,202 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __tsid_python_contact_two_frames_hpp__
+#define __tsid_python_contact_two_frames_hpp__
+
+#include "tsid/bindings/python/fwd.hpp"
+
+#include "tsid/contacts/contact-two-frames.hpp"
+#include "tsid/robots/robot-wrapper.hpp"
+#include "tsid/math/constraint-inequality.hpp"
+#include "tsid/math/constraint-equality.hpp"
+#include "tsid/math/constraint-base.hpp"
+
+namespace tsid {
+namespace python {
+namespace bp = boost::python;
+
+template <typename ContactTwoFrames>
+struct ContactTwoFramesPythonVisitor
+    : public boost::python::def_visitor<
+          ContactTwoFramesPythonVisitor<ContactTwoFrames> > {
+  template <class PyClass>
+
+  void visit(PyClass& cl) const {
+    cl
+        .def(bp::init<std::string, robots::RobotWrapper&, std::string,
+                      std::string, double, double>(
+            (bp::arg("name"), bp::arg("robot"), bp::arg("framename1"),
+             bp::arg("framename2"), bp::arg("minForce"), bp::arg("maxForce")),
+            "Default Constructor"))
+        .add_property("n_motion", &ContactTwoFrames::n_motion,
+                      "return number of motion")
+        .add_property("n_force", &ContactTwoFrames::n_force,
+                      "return number of force")
+        .add_property("name", &ContactTwoFramesPythonVisitor::name,
+                      "return name")
+        .def("computeMotionTask",
+             &ContactTwoFramesPythonVisitor::computeMotionTask,
+             bp::args("t", "q", "v", "data"))
+        .def("computeForceTask",
+             &ContactTwoFramesPythonVisitor::computeForceTask,
+             bp::args("t", "q", "v", "data"))
+        .def("computeForceRegularizationTask",
+             &ContactTwoFramesPythonVisitor::computeForceRegularizationTask,
+             bp::args("t", "q", "v", "data"))
+
+        .add_property(
+            "getForceGeneratorMatrix",
+            bp::make_function(
+                &ContactTwoFramesPythonVisitor::getForceGeneratorMatrix,
+                bp::return_value_policy<bp::copy_const_reference>()))
+
+        .def("getNormalForce", &ContactTwoFramesPythonVisitor::getNormalForce,
+             bp::arg("vec"))
+        .add_property("getMinNormalForce", &ContactTwoFrames::getMinNormalForce)
+        .add_property("getMaxNormalForce", &ContactTwoFrames::getMaxNormalForce)
+
+        .add_property("Kp",
+                      bp::make_function(
+                          &ContactTwoFramesPythonVisitor::Kp,
+                          bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("Kd",
+                      bp::make_function(
+                          &ContactTwoFramesPythonVisitor::Kd,
+                          bp::return_value_policy<bp::copy_const_reference>()))
+        .def("setKp", &ContactTwoFramesPythonVisitor::setKp, bp::arg("Kp"))
+        .def("setKd", &ContactTwoFramesPythonVisitor::setKd, bp::arg("Kd"))
+
+        .def("useLocalFrame", &ContactTwoFramesPythonVisitor::useLocalFrame,
+             bp::arg("local_frame"))
+
+        .def("setContactNormal",
+             &ContactTwoFramesPythonVisitor::setContactNormal, bp::args("vec"))
+        .def("setFrictionCoefficient",
+             &ContactTwoFramesPythonVisitor::setFrictionCoefficient,
+             bp::args("friction_coeff"))
+        .def("setMinNormalForce",
+             &ContactTwoFramesPythonVisitor::setMinNormalForce,
+             bp::args("min_force"))
+        .def("setMaxNormalForce",
+             &ContactTwoFramesPythonVisitor::setMaxNormalForce,
+             bp::args("max_force"))
+        .def("setReference", &ContactTwoFramesPythonVisitor::setReference,
+             bp::args("SE3"))
+        .def("setForceReference",
+             &ContactTwoFramesPythonVisitor::setForceReference,
+             bp::args("f_vec"))
+        .def("setRegularizationTaskWeightVector",
+             &ContactTwoFramesPythonVisitor::setRegularizationTaskWeightVector,
+             bp::args("w_vec"));
+  }
+  static std::string name(ContactTwoFrames& self) {
+    std::string name = self.name();
+    return name;
+  }
+
+  static math::ConstraintEquality computeMotionTask(ContactTwoFrames& self,
+                                                    const double t,
+                                                    const Eigen::VectorXd& q,
+                                                    const Eigen::VectorXd& v,
+                                                    pinocchio::Data& data) {
+    self.computeMotionTask(t, q, v, data);
+    math::ConstraintEquality cons(self.getMotionConstraint().name(),
+                                  self.getMotionConstraint().matrix(),
+                                  self.getMotionConstraint().vector());
+    return cons;
+  }
+  static math::ConstraintInequality computeForceTask(
+      ContactTwoFrames& self, const double t, const Eigen::VectorXd& q,
+      const Eigen::VectorXd& v, const pinocchio::Data& data) {
+    self.computeForceTask(t, q, v, data);
+    math::ConstraintInequality cons(self.getForceConstraint().name(),
+                                    self.getForceConstraint().matrix(),
+                                    self.getForceConstraint().lowerBound(),
+                                    self.getForceConstraint().upperBound());
+    return cons;
+  }
+  static math::ConstraintEquality computeForceRegularizationTask(
+      ContactTwoFrames& self, const double t, const Eigen::VectorXd& q,
+      const Eigen::VectorXd& v, const pinocchio::Data& data) {
+    self.computeForceRegularizationTask(t, q, v, data);
+    math::ConstraintEquality cons(self.getForceRegularizationTask().name(),
+                                  self.getForceRegularizationTask().matrix(),
+                                  self.getForceRegularizationTask().vector());
+    return cons;
+  }
+
+  static void useLocalFrame(ContactTwoFrames& self, const bool local_frame) {
+    self.useLocalFrame(local_frame);
+  }
+  static const Eigen::MatrixXd& getForceGeneratorMatrix(
+      ContactTwoFrames& self) {
+    return self.getForceGeneratorMatrix();
+  }
+  static const Eigen::VectorXd& Kp(ContactTwoFrames& self) { return self.Kp(); }
+  static const Eigen::VectorXd& Kd(ContactTwoFrames& self) { return self.Kd(); }
+  static void setKp(ContactTwoFrames& self, const ::Eigen::VectorXd Kp) {
+    return self.Kp(Kp);
+  }
+  static void setKd(ContactTwoFrames& self, const ::Eigen::VectorXd Kd) {
+    return self.Kd(Kd);
+  }
+  static bool setContactTwoFramess(ContactTwoFrames& self,
+                                   const ::Eigen::MatrixXd ContactTwoFramess) {
+    return self.setContactTwoFramess(ContactTwoFramess);
+  }
+  static bool setContactNormal(ContactTwoFrames& self,
+                               const ::Eigen::VectorXd contactNormal) {
+    return self.setContactNormal(contactNormal);
+  }
+  static bool setFrictionCoefficient(ContactTwoFrames& self,
+                                     const double frictionCoefficient) {
+    return self.setFrictionCoefficient(frictionCoefficient);
+  }
+  static bool setMinNormalForce(ContactTwoFrames& self,
+                                const double minNormalForce) {
+    return self.setMinNormalForce(minNormalForce);
+  }
+  static bool setMaxNormalForce(ContactTwoFrames& self,
+                                const double maxNormalForce) {
+    return self.setMaxNormalForce(maxNormalForce);
+  }
+  static void setReference(ContactTwoFrames& self, const pinocchio::SE3& ref) {
+    self.setReference(ref);
+  }
+  static void setForceReference(ContactTwoFrames& self,
+                                const ::Eigen::VectorXd f_ref) {
+    self.setForceReference(f_ref);
+  }
+  static void setRegularizationTaskWeightVector(ContactTwoFrames& self,
+                                                const ::Eigen::VectorXd w) {
+    self.setRegularizationTaskWeightVector(w);
+  }
+  static double getNormalForce(ContactTwoFrames& self, Eigen::VectorXd f) {
+    return self.getNormalForce(f);
+  }
+
+  static void expose(const std::string& class_name) {
+    std::string doc = "ContactTwoFrames info.";
+    bp::class_<ContactTwoFrames>(class_name.c_str(), doc.c_str(), bp::no_init)
+        .def(ContactTwoFramesPythonVisitor<ContactTwoFrames>());
+  }
+};
+}  // namespace python
+}  // namespace tsid
+
+#endif  // ifndef __tsid_python_contact_two_frames_hpp__
diff --git a/include/tsid/bindings/python/contacts/expose-contact.hpp b/include/tsid/bindings/python/contacts/expose-contact.hpp
index 0db77646..01efac41 100644
--- a/include/tsid/bindings/python/contacts/expose-contact.hpp
+++ b/include/tsid/bindings/python/contacts/expose-contact.hpp
@@ -20,15 +20,18 @@
 
 #include "tsid/bindings/python/contacts/contact-6d.hpp"
 #include "tsid/bindings/python/contacts/contact-point.hpp"
+#include "tsid/bindings/python/contacts/contact-two-frame-positions.hpp"
 
 namespace tsid {
 namespace python {
 void exposeContact6d();
 void exposeContactPoint();
+void exposeContactTwoFramePositions();
 
 inline void exposeContact() {
   exposeContact6d();
   exposeContactPoint();
+  exposeContactTwoFramePositions();
 }
 
 }  // namespace python
diff --git a/include/tsid/bindings/python/formulations/formulation.hpp b/include/tsid/bindings/python/formulations/formulation.hpp
index a0992329..59652bfa 100644
--- a/include/tsid/bindings/python/formulations/formulation.hpp
+++ b/include/tsid/bindings/python/formulations/formulation.hpp
@@ -26,6 +26,7 @@
 #include "tsid/bindings/python/solvers/HQPData.hpp"
 #include "tsid/contacts/contact-6d.hpp"
 #include "tsid/contacts/contact-point.hpp"
+#include "tsid/contacts/contact-two-frame-positions.hpp"
 #include "tsid/tasks/task-joint-posture.hpp"
 #include "tsid/tasks/task-se3-equality.hpp"
 #include "tsid/tasks/task-com-equality.hpp"
@@ -34,6 +35,7 @@
 #include "tsid/tasks/task-joint-bounds.hpp"
 #include "tsid/tasks/task-joint-posVelAcc-bounds.hpp"
 #include "tsid/tasks/task-angular-momentum-equality.hpp"
+#include "tsid/tasks/task-two-frames-equality.hpp"
 
 namespace tsid {
 namespace python {
@@ -65,6 +67,9 @@ struct InvDynPythonVisitor
              bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM,
              bp::args("task", "weight", "priorityLevel", "transition duration"))
+        .def("addMotionTask",
+             &InvDynPythonVisitor::addMotionTask_TwoFramesEquality,
+             bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addForceTask", &InvDynPythonVisitor::addForceTask_COP,
              bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds,
@@ -96,6 +101,14 @@ struct InvDynPythonVisitor
              &InvDynPythonVisitor::addRigidContactPointWithPriorityLevel,
              bp::args("contact", "force_reg_weight", "motion_weight",
                       "priority_level"))
+        .def("addRigidContact",
+             &InvDynPythonVisitor::addRigidContactTwoFramePositions,
+             bp::args("contact", "force_reg_weight"))
+        .def("addRigidContact",
+             &InvDynPythonVisitor::
+                 addRigidContactTwoFramePositionsWithPriorityLevel,
+             bp::args("contact", "force_reg_weight", "motion_weight",
+                      "priority_level"))
         .def("removeTask", &InvDynPythonVisitor::removeTask,
              bp::args("task_name", "duration"))
         .def("removeRigidContact", &InvDynPythonVisitor::removeRigidContact,
@@ -151,6 +164,11 @@ struct InvDynPythonVisitor
                                double transition_duration) {
     return self.addMotionTask(task, weight, priorityLevel, transition_duration);
   }
+  static bool addMotionTask_TwoFramesEquality(
+      T& self, tasks::TaskTwoFramesEquality& task, double weight,
+      unsigned int priorityLevel, double transition_duration) {
+    return self.addMotionTask(task, weight, priorityLevel, transition_duration);
+  }
   static bool addForceTask_COP(T& self, tasks::TaskCopEquality& task,
                                double weight, unsigned int priorityLevel,
                                double transition_duration) {
@@ -203,6 +221,18 @@ struct InvDynPythonVisitor
     return self.addRigidContact(contact, force_regularization_weight,
                                 motion_weight, priority_level);
   }
+  static bool addRigidContactTwoFramePositions(
+      T& self, contacts::ContactTwoFramePositions& contact,
+      double force_regularization_weight) {
+    return self.addRigidContact(contact, force_regularization_weight);
+  }
+  static bool addRigidContactTwoFramePositionsWithPriorityLevel(
+      T& self, contacts::ContactTwoFramePositions& contact,
+      double force_regularization_weight, double motion_weight,
+      const bool priority_level) {
+    return self.addRigidContact(contact, force_regularization_weight,
+                                motion_weight, priority_level);
+  }
   static bool removeTask(T& self, const std::string& task_name,
                          double transition_duration) {
     return self.removeTask(task_name, transition_duration);
diff --git a/include/tsid/bindings/python/tasks/expose-tasks.hpp b/include/tsid/bindings/python/tasks/expose-tasks.hpp
index ebcd4176..ae2a67fe 100644
--- a/include/tsid/bindings/python/tasks/expose-tasks.hpp
+++ b/include/tsid/bindings/python/tasks/expose-tasks.hpp
@@ -26,6 +26,7 @@
 #include "tsid/bindings/python/tasks/task-joint-bounds.hpp"
 #include "tsid/bindings/python/tasks/task-joint-posVelAcc-bounds.hpp"
 #include "tsid/bindings/python/tasks/task-am-equality.hpp"
+#include "tsid/bindings/python/tasks/task-two-frames-equality.hpp"
 
 namespace tsid {
 namespace python {
@@ -37,6 +38,7 @@ void exposeTaskActuationBounds();
 void exposeTaskJointBounds();
 void exposeTaskJointPosVelAccBounds();
 void exposeTaskAMEquality();
+void exposeTaskTwoFramesEquality();
 
 inline void exposeTasks() {
   exposeTaskComEquality();
@@ -47,6 +49,7 @@ inline void exposeTasks() {
   exposeTaskJointBounds();
   exposeTaskJointPosVelAccBounds();
   exposeTaskAMEquality();
+  exposeTaskTwoFramesEquality();
 }
 
 }  // namespace python
diff --git a/include/tsid/bindings/python/tasks/task-two-frames-equality.hpp b/include/tsid/bindings/python/tasks/task-two-frames-equality.hpp
new file mode 100644
index 00000000..fdcef4b8
--- /dev/null
+++ b/include/tsid/bindings/python/tasks/task-two-frames-equality.hpp
@@ -0,0 +1,149 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __tsid_python_task_frames_hpp__
+#define __tsid_python_task_frames_hpp__
+
+#include "tsid/bindings/python/fwd.hpp"
+
+#include "tsid/tasks/task-two-frames-equality.hpp"
+#include "tsid/robots/robot-wrapper.hpp"
+#include "tsid/trajectories/trajectory-base.hpp"
+#include "tsid/math/constraint-equality.hpp"
+#include "tsid/math/constraint-base.hpp"
+namespace tsid {
+namespace python {
+namespace bp = boost::python;
+
+template <typename TaskFrames>
+struct TaskTwoFramesEqualityPythonVisitor
+    : public boost::python::def_visitor<
+          TaskTwoFramesEqualityPythonVisitor<TaskFrames> > {
+  template <class PyClass>
+
+  void visit(PyClass& cl) const {
+    cl.def(bp::init<std::string, robots::RobotWrapper&, std::string,
+                    std::string>((bp::arg("name"), bp::arg("robot"),
+                                  bp::arg("framename1"), bp::arg("framename2")),
+                                 "Default Constructor"))
+        .add_property("dim", &TaskFrames::dim, "return dimension size")
+        .add_property(
+            "getDesiredAcceleration",
+            bp::make_function(
+                &TaskTwoFramesEqualityPythonVisitor::getDesiredAcceleration,
+                bp::return_value_policy<bp::copy_const_reference>()),
+            "Return Acc_desired")
+        .def("getAcceleration",
+             &TaskTwoFramesEqualityPythonVisitor::getAcceleration,
+             bp::arg("dv"))
+        .add_property("position_error",
+                      bp::make_function(
+                          &TaskTwoFramesEqualityPythonVisitor::position_error,
+                          bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("velocity_error",
+                      bp::make_function(
+                          &TaskTwoFramesEqualityPythonVisitor::velocity_error,
+                          bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("Kp",
+                      bp::make_function(
+                          &TaskTwoFramesEqualityPythonVisitor::Kp,
+                          bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("Kd",
+                      bp::make_function(
+                          &TaskTwoFramesEqualityPythonVisitor::Kd,
+                          bp::return_value_policy<bp::copy_const_reference>()))
+        .def("setKp", &TaskTwoFramesEqualityPythonVisitor::setKp, bp::arg("Kp"))
+        .def("setKd", &TaskTwoFramesEqualityPythonVisitor::setKd, bp::arg("Kd"))
+        .add_property("mask",
+                      bp::make_function(
+                          &TaskTwoFramesEqualityPythonVisitor::getMask,
+                          bp::return_value_policy<bp::copy_const_reference>()),
+                      "Return mask")
+        .def("setMask", &TaskTwoFramesEqualityPythonVisitor::setMask,
+             bp::arg("mask"))
+        .def("compute", &TaskTwoFramesEqualityPythonVisitor::compute,
+             bp::args("t", "q", "v", "data"))
+        .def("getConstraint",
+             &TaskTwoFramesEqualityPythonVisitor::getConstraint)
+        .add_property("frame_id1", &TaskFrames::frame_id1, "frame id 1 return")
+        .add_property("frame_id2", &TaskFrames::frame_id2, "frame id 2 return")
+        .add_property("name", &TaskTwoFramesEqualityPythonVisitor::name);
+  }
+  static std::string name(TaskFrames& self) {
+    std::string name = self.name();
+    return name;
+  }
+  static math::ConstraintEquality compute(TaskFrames& self, const double t,
+                                          const Eigen::VectorXd& q,
+                                          const Eigen::VectorXd& v,
+                                          pinocchio::Data& data) {
+    self.compute(t, q, v, data);
+    math::ConstraintEquality cons(self.getConstraint().name(),
+                                  self.getConstraint().matrix(),
+                                  self.getConstraint().vector());
+    return cons;
+  }
+  static math::ConstraintEquality getConstraint(const TaskFrames& self) {
+    math::ConstraintEquality cons(self.getConstraint().name(),
+                                  self.getConstraint().matrix(),
+                                  self.getConstraint().vector());
+    return cons;
+  }
+  static const Eigen::VectorXd& getDesiredAcceleration(const TaskFrames& self) {
+    return self.getDesiredAcceleration();
+  }
+  static Eigen::VectorXd getAcceleration(TaskFrames& self,
+                                         const Eigen::VectorXd dv) {
+    return self.getAcceleration(dv);
+  }
+  static const Eigen::VectorXd& position_error(const TaskFrames& self) {
+    return self.position_error();
+  }
+  static const Eigen::VectorXd& velocity_error(const TaskFrames& self) {
+    return self.velocity_error();
+  }
+  static const Eigen::VectorXd& Kp(TaskFrames& self) { return self.Kp(); }
+  static const Eigen::VectorXd& Kd(TaskFrames& self) { return self.Kd(); }
+  static void setKp(TaskFrames& self, const ::Eigen::VectorXd Kp) {
+    return self.Kp(Kp);
+  }
+  static void setKd(TaskFrames& self, const ::Eigen::VectorXd Kv) {
+    return self.Kd(Kv);
+  }
+  static void getMask(TaskFrames& self) { self.getMask(); }
+  static void setMask(TaskFrames& self, const ::Eigen::VectorXd mask) {
+    self.setMask(mask);
+  }
+  static Eigen::VectorXd frame_id1(TaskFrames& self) {
+    return self.frame_id1();
+  }
+  static Eigen::VectorXd frame_id2(TaskFrames& self) {
+    return self.frame_id2();
+  }
+
+  static void expose(const std::string& class_name) {
+    std::string doc = "TaskFrames info.";
+    bp::class_<TaskFrames>(class_name.c_str(), doc.c_str(), bp::no_init)
+        .def(TaskTwoFramesEqualityPythonVisitor<TaskFrames>());
+
+    //  bp::register_ptr_to_python< boost::shared_ptr<math::ConstraintBase> >();
+  }
+};
+}  // namespace python
+}  // namespace tsid
+
+#endif  // ifndef __tsid_python_task_frames_hpp__
diff --git a/include/tsid/contacts/contact-base.hpp b/include/tsid/contacts/contact-base.hpp
index f683ea7f..f1f887fd 100644
--- a/include/tsid/contacts/contact-base.hpp
+++ b/include/tsid/contacts/contact-base.hpp
@@ -39,6 +39,7 @@ class ContactBase {
   typedef math::Matrix Matrix;
   typedef math::Matrix3x Matrix3x;
   typedef tasks::TaskSE3Equality TaskSE3Equality;
+  typedef tasks::TaskMotion TaskMotion;
   typedef pinocchio::Data Data;
   typedef robots::RobotWrapper RobotWrapper;
 
@@ -71,7 +72,7 @@ class ContactBase {
   virtual const ConstraintEquality& computeForceRegularizationTask(
       const double t, ConstRefVector q, ConstRefVector v, const Data& data) = 0;
 
-  virtual const TaskSE3Equality& getMotionTask() const = 0;
+  virtual const TaskMotion& getMotionTask() const = 0;
   virtual const ConstraintBase& getMotionConstraint() const = 0;
   virtual const ConstraintInequality& getForceConstraint() const = 0;
   virtual const ConstraintEquality& getForceRegularizationTask() const = 0;
diff --git a/include/tsid/contacts/contact-two-frame-positions.hpp b/include/tsid/contacts/contact-two-frame-positions.hpp
new file mode 100644
index 00000000..890ac5f5
--- /dev/null
+++ b/include/tsid/contacts/contact-two-frame-positions.hpp
@@ -0,0 +1,119 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_contact_two_frame_positions_hpp__
+#define __invdyn_contact_two_frame_positions_hpp__
+
+#include "tsid/contacts/contact-base.hpp"
+#include "tsid/tasks/task-se3-equality.hpp"
+#include "tsid/tasks/task-two-frames-equality.hpp"
+#include "tsid/math/constraint-inequality.hpp"
+#include "tsid/math/constraint-equality.hpp"
+
+namespace tsid {
+namespace contacts {
+class ContactTwoFramePositions : public ContactBase {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef math::ConstRefMatrix ConstRefMatrix;
+  typedef math::ConstRefVector ConstRefVector;
+  typedef math::Matrix3x Matrix3x;
+  typedef math::Vector6 Vector6;
+  typedef math::Vector3 Vector3;
+  typedef math::Vector Vector;
+  typedef tasks::TaskTwoFramesEquality TaskTwoFramesEquality;
+  typedef tasks::TaskSE3Equality TaskSE3Equality;
+  typedef math::ConstraintInequality ConstraintInequality;
+  typedef math::ConstraintEquality ConstraintEquality;
+  typedef pinocchio::SE3 SE3;
+
+  ContactTwoFramePositions(const std::string& name, RobotWrapper& robot,
+                           const std::string& frameName1,
+                           const std::string& frameName2,
+                           const double minNormalForce,
+                           const double maxNormalForce);
+
+  /// Return the number of motion constraints
+  virtual unsigned int n_motion() const;
+
+  /// Return the number of force variables
+  virtual unsigned int n_force() const;
+
+  virtual const ConstraintBase& computeMotionTask(const double t,
+                                                  ConstRefVector q,
+                                                  ConstRefVector v, Data& data);
+
+  virtual const ConstraintInequality& computeForceTask(const double t,
+                                                       ConstRefVector q,
+                                                       ConstRefVector v,
+                                                       const Data& data);
+
+  virtual const Matrix& getForceGeneratorMatrix();
+
+  virtual const ConstraintEquality& computeForceRegularizationTask(
+      const double t, ConstRefVector q, ConstRefVector v, const Data& data);
+
+  const TaskTwoFramesEquality& getMotionTask() const;
+  const ConstraintBase& getMotionConstraint() const;
+  const ConstraintInequality& getForceConstraint() const;
+  const ConstraintEquality& getForceRegularizationTask() const;
+  double getMotionTaskWeight() const;
+  const Matrix3x& getContactPoints() const;
+
+  double getNormalForce(ConstRefVector f) const;
+  double getMinNormalForce() const;
+  double getMaxNormalForce() const;
+
+  const Vector&
+  Kp();  // cannot be const because it set a member variable inside
+  const Vector&
+  Kd();  // cannot be const because it set a member variable inside
+  void Kp(ConstRefVector Kp);
+  void Kd(ConstRefVector Kp);
+
+  bool setContactNormal(ConstRefVector contactNormal);
+
+  bool setFrictionCoefficient(const double frictionCoefficient);
+  bool setMinNormalForce(const double minNormalForce);
+  bool setMaxNormalForce(const double maxNormalForce);
+  bool setMotionTaskWeight(const double w);
+  void setForceReference(ConstRefVector& f_ref);
+  void setRegularizationTaskWeightVector(ConstRefVector& w);
+
+ protected:
+  void updateForceInequalityConstraints();
+  void updateForceRegularizationTask();
+  void updateForceGeneratorMatrix();
+
+  TaskTwoFramesEquality m_motionTask;
+  ConstraintInequality m_forceInequality;
+  ConstraintEquality m_forceRegTask;
+  Vector3 m_fRef;
+  Vector3 m_weightForceRegTask;
+  Matrix3x m_contactPoints;
+  Vector m_Kp3, m_Kd3;  // gain vectors to be returned by reference
+  double m_fMin;
+  double m_fMax;
+  double m_regularizationTaskWeight;
+  double m_motionTaskWeight;
+  Matrix m_forceGenMat;
+};
+}  // namespace contacts
+}  // namespace tsid
+
+#endif  // ifndef __invdyn_contact_two_frame_positions_hpp__
diff --git a/include/tsid/tasks/task-two-frames-equality.hpp b/include/tsid/tasks/task-two-frames-equality.hpp
new file mode 100644
index 00000000..696d7de5
--- /dev/null
+++ b/include/tsid/tasks/task-two-frames-equality.hpp
@@ -0,0 +1,123 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_task_frames_equality_hpp__
+#define __invdyn_task_frames_equality_hpp__
+
+#include "tsid/tasks/task-motion.hpp"
+#include "tsid/trajectories/trajectory-base.hpp"
+#include "tsid/math/constraint-equality.hpp"
+
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/multibody/data.hpp>
+
+namespace tsid {
+namespace tasks {
+
+class TaskTwoFramesEquality : public TaskMotion {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef math::Index Index;
+  typedef trajectories::TrajectorySample TrajectorySample;
+  typedef math::Vector Vector;
+  typedef math::ConstraintEquality ConstraintEquality;
+  typedef pinocchio::Data Data;
+  typedef pinocchio::Data::Matrix6x Matrix6x;
+  typedef pinocchio::Motion Motion;
+  typedef pinocchio::SE3 SE3;
+
+  TaskTwoFramesEquality(const std::string& name, RobotWrapper& robot,
+                        const std::string& frameName1,
+                        const std::string& frameName2);
+
+  int dim() const;
+
+  const ConstraintBase& compute(const double t, ConstRefVector q,
+                                ConstRefVector v, Data& data);
+
+  const ConstraintBase& getConstraint() const;
+
+  /** Return the desired task acceleration (after applying the specified mask).
+   *  The value is expressed in local frame is the local_frame flag is true,
+   *  otherwise it is expressed in a local world-oriented frame.
+   */
+  const Vector& getDesiredAcceleration() const;
+
+  /** Return the task acceleration (after applying the specified mask).
+   *  The value is expressed in local frame is the local_frame flag is true,
+   *  otherwise it is expressed in a local world-oriented frame.
+   */
+  Vector getAcceleration(ConstRefVector dv) const;
+
+  virtual void setMask(math::ConstRefVector mask);
+
+  /** Return the position tracking error (after applying the specified mask).
+   *  The error is expressed in local frame is the local_frame flag is true,
+   *  otherwise it is expressed in a local world-oriented frame.
+   */
+  const Vector& position_error() const;
+
+  /** Return the velocity tracking error (after applying the specified mask).
+   *  The error is expressed in local frame is the local_frame flag is true,
+   *  otherwise it is expressed in a local world-oriented frame.
+   */
+  const Vector& velocity_error() const;
+
+  const Vector& Kp() const;
+  const Vector& Kd() const;
+  void Kp(ConstRefVector Kp);
+  void Kd(ConstRefVector Kp);
+
+  Index frame_id1() const;
+  Index frame_id2() const;
+
+  /**
+   * @brief Specifies if the jacobian and desired acceloration should be
+   * expressed in the local frame or the local world-oriented frame.
+   *
+   * @param local_frame If true, represent jacobian and acceloration in the
+   *   local frame. If false, represent them in the local world-oriented frame.
+   */
+  void useLocalFrame(bool local_frame);
+
+ protected:
+  std::string m_frame_name1;
+  std::string m_frame_name2;
+  Index m_frame_id1;
+  Index m_frame_id2;
+  Motion m_p_error, m_v_error;
+  Vector m_p_error_vec, m_v_error_vec;
+  Vector m_p_error_masked_vec, m_v_error_masked_vec;
+  Vector m_p, m_v;
+  Vector m_p_ref, m_v_ref_vec;
+  Motion m_v_ref, m_a_ref;
+  SE3 m_wMl1, m_wMl2;
+  Vector m_Kp;
+  Vector m_Kd;
+  Vector m_a_des, m_a_des_masked;
+  Motion m_drift;
+  Vector m_drift_masked;
+  Matrix6x m_J1, m_J2;
+  Matrix6x m_J1_rotated, m_J2_rotated;
+  ConstraintEquality m_constraint;
+};
+
+}  // namespace tasks
+}  // namespace tsid
+
+#endif  // ifndef __invdyn_task_frames_equality_hpp__
diff --git a/src/contacts/contact-two-frame-positions.cpp b/src/contacts/contact-two-frame-positions.cpp
new file mode 100644
index 00000000..9a8e0c95
--- /dev/null
+++ b/src/contacts/contact-two-frame-positions.cpp
@@ -0,0 +1,193 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include "tsid/math/utils.hpp"
+#include "tsid/contacts/contact-two-frame-positions.hpp"
+
+#include <pinocchio/spatial/skew.hpp>
+
+using namespace tsid;
+using namespace contacts;
+using namespace math;
+using namespace trajectories;
+using namespace tasks;
+
+ContactTwoFramePositions::ContactTwoFramePositions(
+    const std::string& name, RobotWrapper& robot, const std::string& frameName1,
+    const std::string& frameName2, const double minNormalForce,
+    const double maxNormalForce)
+    : ContactBase(name, robot),
+      m_motionTask(
+          name, robot, frameName1,
+          frameName2),  // Actual motion task with type TaskTwoFramesEquality
+      m_forceInequality(name, 3, 3),
+      m_forceRegTask(name, 3, 3),
+      m_fMin(minNormalForce),
+      m_fMax(maxNormalForce) {
+  m_weightForceRegTask << 1, 1, 1;
+  m_forceGenMat.resize(3, 3);
+  m_fRef = Vector3::Zero();
+  m_contactPoints.resize(3, 1);
+  m_contactPoints.setZero();
+  updateForceGeneratorMatrix();
+  updateForceInequalityConstraints();
+  updateForceRegularizationTask();
+
+  // This contact has forceGenMat as 3x3 identity matrix, so it can be used only
+  // for emulating a ball joint between two frames The forces calculated will
+  // have only linear part (rotation will be unconstrained) So we need to set
+  // the appropriate mask for motion task (which can take into account rotation
+  // but we don't need it)
+  math::Vector motion_mask(6);
+  motion_mask << 1., 1., 1., 0., 0., 0.;
+  m_motionTask.setMask(motion_mask);
+}
+
+void ContactTwoFramePositions::updateForceInequalityConstraints() {
+  Matrix B = Matrix::Identity(3, 3);  // Force "gluing" two frames together can
+                                      // be arbitrary in sign/direction
+  Vector lb = m_fMin * Vector::Ones(3);
+  Vector ub = m_fMax * Vector::Ones(3);
+
+  m_forceInequality.setMatrix(B);
+  m_forceInequality.setLowerBound(lb);
+  m_forceInequality.setUpperBound(ub);
+}
+
+double ContactTwoFramePositions::getNormalForce(ConstRefVector f) const {
+  return 0.0;
+}
+
+const Matrix3x& ContactTwoFramePositions::getContactPoints() const {
+  return m_contactPoints;
+}
+
+void ContactTwoFramePositions::setRegularizationTaskWeightVector(
+    ConstRefVector& w) {
+  m_weightForceRegTask = w;
+  updateForceRegularizationTask();
+}
+
+void ContactTwoFramePositions::updateForceRegularizationTask() {
+  typedef Eigen::Matrix<double, 3, 3> Matrix3;
+  Matrix3 A = Matrix3::Zero();
+  A.diagonal() = m_weightForceRegTask;
+  m_forceRegTask.setMatrix(A);
+  m_forceRegTask.setVector(A * m_fRef);
+}
+
+void ContactTwoFramePositions::updateForceGeneratorMatrix() {
+  m_forceGenMat.setIdentity();
+}
+
+unsigned int ContactTwoFramePositions::n_motion() const {
+  return m_motionTask.dim();
+}
+unsigned int ContactTwoFramePositions::n_force() const { return 3; }
+
+const Vector& ContactTwoFramePositions::Kp() {
+  m_Kp3 = m_motionTask.Kp().head<3>();
+  return m_Kp3;
+}
+
+const Vector& ContactTwoFramePositions::Kd() {
+  m_Kd3 = m_motionTask.Kd().head<3>();
+  return m_Kd3;
+}
+
+void ContactTwoFramePositions::Kp(ConstRefVector Kp) {
+  assert(Kp.size() == 3);
+  Vector6 Kp6;
+  Kp6.head<3>() = Kp;
+  m_motionTask.Kp(Kp6);
+}
+
+void ContactTwoFramePositions::Kd(ConstRefVector Kd) {
+  assert(Kd.size() == 3);
+  Vector6 Kd6;
+  Kd6.head<3>() = Kd;
+  m_motionTask.Kd(Kd6);
+}
+
+bool ContactTwoFramePositions::setContactNormal(ConstRefVector contactNormal) {
+  return true;
+}
+
+bool ContactTwoFramePositions::setFrictionCoefficient(
+    const double frictionCoefficient) {
+  return true;
+}
+
+bool ContactTwoFramePositions::setMinNormalForce(const double minNormalForce) {
+  m_fMin = minNormalForce;
+  updateForceInequalityConstraints();
+  return true;
+}
+
+bool ContactTwoFramePositions::setMaxNormalForce(const double maxNormalForce) {
+  m_fMax = maxNormalForce;
+  updateForceInequalityConstraints();
+  return true;
+}
+
+void ContactTwoFramePositions::setForceReference(ConstRefVector& f_ref) {
+  m_fRef = f_ref;
+  updateForceRegularizationTask();
+}
+
+const ConstraintBase& ContactTwoFramePositions::computeMotionTask(
+    const double t, ConstRefVector q, ConstRefVector v, Data& data) {
+  return m_motionTask.compute(t, q, v, data);
+}
+
+const ConstraintInequality& ContactTwoFramePositions::computeForceTask(
+    const double, ConstRefVector, ConstRefVector, const Data&) {
+  return m_forceInequality;
+}
+
+const Matrix& ContactTwoFramePositions::getForceGeneratorMatrix() {
+  return m_forceGenMat;
+}
+
+const ConstraintEquality&
+ContactTwoFramePositions::computeForceRegularizationTask(const double,
+                                                         ConstRefVector,
+                                                         ConstRefVector,
+                                                         const Data&) {
+  return m_forceRegTask;
+}
+
+double ContactTwoFramePositions::getMinNormalForce() const { return m_fMin; }
+double ContactTwoFramePositions::getMaxNormalForce() const { return m_fMax; }
+
+const TaskTwoFramesEquality& ContactTwoFramePositions::getMotionTask() const {
+  return m_motionTask;
+}
+
+const ConstraintBase& ContactTwoFramePositions::getMotionConstraint() const {
+  return m_motionTask.getConstraint();
+}
+
+const ConstraintInequality& ContactTwoFramePositions::getForceConstraint()
+    const {
+  return m_forceInequality;
+}
+
+const ConstraintEquality& ContactTwoFramePositions::getForceRegularizationTask()
+    const {
+  return m_forceRegTask;
+}
diff --git a/src/tasks/task-cop-equality.cpp b/src/tasks/task-cop-equality.cpp
index 27be4418..3c362642 100644
--- a/src/tasks/task-cop-equality.cpp
+++ b/src/tasks/task-cop-equality.cpp
@@ -65,7 +65,11 @@ const ConstraintBase& TaskCopEquality::compute(const double, ConstRefVector,
     unsigned int i = cl->index;
     // get contact points in local frame and transform them to world frame
     const Matrix3x& P = cl->contact.getContactPoints();
-    m_robot.framePosition(data, cl->contact.getMotionTask().frame_id(), oMi);
+    m_robot.framePosition(
+        data,
+        static_cast<const TaskSE3Equality&>(cl->contact.getMotionTask())
+            .frame_id(),
+        oMi);
     // cout<<"Contact "<<cl->contact.name()<<endl;
     // cout<<"oMi\n"<<oMi<<endl;
     for (int j = 0; j < P.cols(); ++j) {
diff --git a/src/tasks/task-two-frames-equality.cpp b/src/tasks/task-two-frames-equality.cpp
new file mode 100644
index 00000000..f0fea047
--- /dev/null
+++ b/src/tasks/task-two-frames-equality.cpp
@@ -0,0 +1,181 @@
+//
+// Copyright (c) 2023 MIPT
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include "tsid/math/utils.hpp"
+#include "tsid/tasks/task-two-frames-equality.hpp"
+#include "tsid/robots/robot-wrapper.hpp"
+
+namespace tsid {
+namespace tasks {
+using namespace std;
+using namespace math;
+using namespace trajectories;
+using namespace pinocchio;
+
+TaskTwoFramesEquality::TaskTwoFramesEquality(const std::string& name,
+                                             RobotWrapper& robot,
+                                             const std::string& frameName1,
+                                             const std::string& frameName2)
+    : TaskMotion(name, robot),
+      m_frame_name1(frameName1),
+      m_frame_name2(frameName2),
+      m_constraint(name, 6, robot.nv()) {
+  assert(m_robot.model().existFrame(frameName1));
+  assert(m_robot.model().existFrame(frameName2));
+  m_frame_id1 = m_robot.model().getFrameId(frameName1);
+  m_frame_id2 = m_robot.model().getFrameId(frameName2);
+
+  m_v_ref.setZero();
+  m_a_ref.setZero();
+  m_wMl1.setIdentity();
+  m_wMl2.setIdentity();
+  m_p_error_vec.setZero(6);
+  m_v_error_vec.setZero(6);
+  m_p.resize(12);
+  m_v.resize(6);
+  m_p_ref.resize(12);
+  m_v_ref_vec.resize(6);
+  m_Kp.setZero(6);
+  m_Kd.setZero(6);
+  m_a_des.setZero(6);
+  m_J1.setZero(6, robot.nv());
+  m_J2.setZero(6, robot.nv());
+  m_J1_rotated.setZero(6, robot.nv());
+  m_J2_rotated.setZero(6, robot.nv());
+
+  m_mask.resize(6);
+  m_mask.fill(1.);
+  setMask(m_mask);
+}
+
+void TaskTwoFramesEquality::setMask(math::ConstRefVector mask) {
+  TaskMotion::setMask(mask);
+  int n = dim();
+  m_constraint.resize(n, (unsigned int)m_J1.cols());
+  m_p_error_masked_vec.resize(n);
+  m_v_error_masked_vec.resize(n);
+  m_drift_masked.resize(n);
+  m_a_des_masked.resize(n);
+}
+
+int TaskTwoFramesEquality::dim() const { return (int)m_mask.sum(); }
+
+const Vector& TaskTwoFramesEquality::Kp() const { return m_Kp; }
+
+const Vector& TaskTwoFramesEquality::Kd() const { return m_Kd; }
+
+void TaskTwoFramesEquality::Kp(ConstRefVector Kp) {
+  assert(Kp.size() == 6);
+  m_Kp = Kp;
+}
+
+void TaskTwoFramesEquality::Kd(ConstRefVector Kd) {
+  assert(Kd.size() == 6);
+  m_Kd = Kd;
+}
+
+const Vector& TaskTwoFramesEquality::position_error() const {
+  return m_p_error_masked_vec;
+}
+
+const Vector& TaskTwoFramesEquality::velocity_error() const {
+  return m_v_error_masked_vec;
+}
+
+const Vector& TaskTwoFramesEquality::getDesiredAcceleration() const {
+  return m_a_des_masked;
+}
+
+Vector TaskTwoFramesEquality::getAcceleration(ConstRefVector dv) const {
+  return m_constraint.matrix() * dv + m_drift_masked;
+}
+
+Index TaskTwoFramesEquality::frame_id1() const { return m_frame_id1; }
+
+Index TaskTwoFramesEquality::frame_id2() const { return m_frame_id2; }
+
+const ConstraintBase& TaskTwoFramesEquality::getConstraint() const {
+  return m_constraint;
+}
+
+const ConstraintBase& TaskTwoFramesEquality::compute(const double,
+                                                     ConstRefVector,
+                                                     ConstRefVector,
+                                                     Data& data) {
+  // Calculating task with formulation: [J1 - J2   0   0] dv = [-J1dot*v +
+  // J2dot*v]
+
+  SE3 oMi1, oMi2;
+  Motion v_frame1, v_frame2;
+  Motion m_drift1, m_drift2;
+  m_robot.framePosition(data, m_frame_id1, oMi1);
+  m_robot.framePosition(data, m_frame_id2, oMi2);
+  m_robot.frameVelocity(data, m_frame_id1, v_frame1);
+  m_robot.frameVelocity(data, m_frame_id2, v_frame2);
+  m_robot.frameClassicAcceleration(data, m_frame_id1, m_drift1);
+  m_robot.frameClassicAcceleration(data, m_frame_id2, m_drift2);
+
+  // Transformations from local to local-world-aligned frame (thus only rotation
+  // is used)
+  m_wMl1.rotation(oMi1.rotation());
+  m_wMl2.rotation(oMi2.rotation());
+
+  m_robot.frameJacobianLocal(data, m_frame_id1, m_J1);
+  m_robot.frameJacobianLocal(data, m_frame_id2, m_J2);
+
+  // Doing all calculations in local-world-aligned frame
+  errorInSE3(oMi1, oMi2, m_p_error);  // pos err in local oMi1 frame
+  m_p_error_vec = m_wMl1.toActionMatrix() *
+                  m_p_error.toVector();  // pos err in local-world-aligned frame
+
+  m_v_error = m_wMl2.act(v_frame2) -
+              m_wMl1.act(v_frame1);  // vel err in local-world-aligned frame
+
+  // desired acc in local-world-aligned frame
+  m_a_des = m_Kp.cwiseProduct(m_p_error_vec) +
+            m_Kd.cwiseProduct(m_v_error.toVector());
+
+  m_v_error_vec = m_v_error.toVector();
+
+  m_drift = (m_wMl1.act(m_drift1) - m_wMl2.act(m_drift2));
+
+  m_J1_rotated.noalias() =
+      m_wMl1.toActionMatrix() *
+      m_J1;  // Use an explicit temporary m_J_rotated to avoid allocations
+  m_J1 = m_J1_rotated;
+
+  m_J2_rotated.noalias() = m_wMl2.toActionMatrix() * m_J2;
+  m_J2 = m_J2_rotated;
+
+  int idx = 0;
+  for (int i = 0; i < 6; i++) {
+    if (m_mask(i) != 1.) continue;
+
+    m_constraint.matrix().row(idx) = m_J1.row(i) - m_J2.row(i);
+    m_constraint.vector().row(idx) = (m_a_des - m_drift.toVector()).row(i);
+    m_a_des_masked(idx) = m_a_des(i);
+    m_drift_masked(idx) = m_drift.toVector()(i);
+    m_p_error_masked_vec(idx) = m_p_error_vec(i);
+    m_v_error_masked_vec(idx) = m_v_error_vec(i);
+
+    idx += 1;
+  }
+
+  return m_constraint;
+}
+}  // namespace tasks
+}  // namespace tsid


Test patch:
diff --git a/tests/python/test_Constraint.py b/tests/python/test_Constraint.py
index 0f4cbae5..7110b517 100644
--- a/tests/python/test_Constraint.py
+++ b/tests/python/test_Constraint.py
@@ -1,6 +1,5 @@
 import numpy as np
 import pinocchio as se3
-
 import tsid
 
 print("")
diff --git a/tests/python/test_Contact.py b/tests/python/test_Contact.py
index 33381911..4872e919 100644
--- a/tests/python/test_Contact.py
+++ b/tests/python/test_Contact.py
@@ -2,7 +2,6 @@
 
 import numpy as np
 import pinocchio as se3
-
 import tsid
 
 print("")
diff --git a/tests/python/test_ContactPoint.py b/tests/python/test_ContactPoint.py
index cdf9e505..00e5f24b 100644
--- a/tests/python/test_ContactPoint.py
+++ b/tests/python/test_ContactPoint.py
@@ -3,7 +3,6 @@
 
 import numpy as np
 import pinocchio as se3
-
 import tsid
 
 print("")
diff --git a/tests/python/test_Deprecations.py b/tests/python/test_Deprecations.py
index 8e0bf828..588ce382 100644
--- a/tests/python/test_Deprecations.py
+++ b/tests/python/test_Deprecations.py
@@ -3,7 +3,6 @@
 import warnings
 
 import numpy as np
-
 import tsid
 
 
diff --git a/tests/python/test_Formulation.py b/tests/python/test_Formulation.py
index 6d993306..07d3af06 100644
--- a/tests/python/test_Formulation.py
+++ b/tests/python/test_Formulation.py
@@ -2,9 +2,8 @@
 
 import numpy as np
 import pinocchio as se3
-from numpy.linalg import norm
-
 import tsid
+from numpy.linalg import norm
 
 print("")
 print("Test InvDyn")
diff --git a/tests/python/test_Gravity.py b/tests/python/test_Gravity.py
index 84072885..c85d4fb8 100644
--- a/tests/python/test_Gravity.py
+++ b/tests/python/test_Gravity.py
@@ -2,7 +2,6 @@
 
 import numpy as np
 import pinocchio as se3
-
 import tsid
 
 print("")
diff --git a/tests/python/test_RobotWrapper.py b/tests/python/test_RobotWrapper.py
index 7fa1df85..6e7bb073 100644
--- a/tests/python/test_RobotWrapper.py
+++ b/tests/python/test_RobotWrapper.py
@@ -2,7 +2,6 @@
 
 import numpy as np
 import pinocchio as se3
-
 import tsid
 
 print("")
diff --git a/tests/python/test_Solvers.py b/tests/python/test_Solvers.py
index 4f437d0e..eb8e2896 100644
--- a/tests/python/test_Solvers.py
+++ b/tests/python/test_Solvers.py
@@ -1,5 +1,4 @@
 import numpy as np
-
 import tsid
 
 print("")
diff --git a/tests/python/test_Tasks.py b/tests/python/test_Tasks.py
index cb98d577..59711dc1 100644
--- a/tests/python/test_Tasks.py
+++ b/tests/python/test_Tasks.py
@@ -2,9 +2,8 @@
 
 import numpy as np
 import pinocchio as pin
-from numpy.linalg import norm
-
 import tsid
+from numpy.linalg import norm
 
 print("")
 print("Test Task COM")
diff --git a/tests/python/test_Trajectories.py b/tests/python/test_Trajectories.py
index 0674e989..8c0acdd5 100644
--- a/tests/python/test_Trajectories.py
+++ b/tests/python/test_Trajectories.py
@@ -1,6 +1,5 @@
 import numpy as np
 import pinocchio as se3
-
 import tsid
 
 print("")