diff --git a/CMakeLists.txt b/CMakeLists.txt
index 06dfa9e2745f5656002af0b87a5d73455fe22b35..6806219ce9576925f24a9d2bdd1c17281296c01c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -159,7 +159,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
@@ -168,7 +169,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
@@ -276,12 +278,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 0000000000000000000000000000000000000000..5a148ccb85efcd9df5e82685d7c1d4dc98d31251
--- /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 0000000000000000000000000000000000000000..08c022279f006e83baea992a04b835b578395215
--- /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_tsid_talos_gripper_closed_kinematic_chain.py b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
new file mode 100644
index 0000000000000000000000000000000000000000..e48918d22f55eea91a8412c384e45c1ecc9e38d8
--- /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/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 0000000000000000000000000000000000000000..f7de131549d0fcc094aa09f85c2e2f510a84f0a2
--- /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 0000000000000000000000000000000000000000..7889efb778f7f25f11cd9645ac90dbafe8025be1
--- /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 0db776469d61e9a6fcc5e6e1ef3424b7668b93bd..01efac415a1d2dd60ef827c0189eaad4d08c5cc2 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 a0992329522fbec9647a773234cddda80e4e8b18..59652bfa67dfcff29ad7c1157c1c65185b8c8572 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 ebcd4176a339313672e7e20020b967a458798a3a..ae2a67fec6bf8cd4cee4f65c0e8a20cd2fa4965d 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 0000000000000000000000000000000000000000..fdcef4b8f63be43bc4a44f162d7ed8fe7a03cdbc
--- /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 f683ea7f5231ca4bbe96d65794a429c0e522ef32..f1f887fd232e246f27d6588f07faba4bd7d60431 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 0000000000000000000000000000000000000000..890ac5f572c08ecdc6a2c1b4168b0b1f016c7237
--- /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 0000000000000000000000000000000000000000..696d7de5d0a38212d020290851d6387178621021
--- /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 0000000000000000000000000000000000000000..9a8e0c9547f6e4411e4566abdf27e285316898e1
--- /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 27be44186414002f35b2543f82355aa5a128ce41..3c362642bc47f22885184e54830a41232df06e80 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 0000000000000000000000000000000000000000..f0fea047c6691c9ffbddcce8897bef0f1e3653fb
--- /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