Two frames contact pr (#218)

* TaskTwoFramesEquality and ContactTwoFramePositions added with appropriate bindings. 

* getMotionTask type changed from TaskSE3Equality to TaskMotion to allow contacts use different motion tasks, not only TaskSE3Equality

* python demo of talos gripper with closed kinematic chain, using URDF/STLs in external repo


---------
Authored-by: @egordv

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

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

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

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

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

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")

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")

exercizes/notebooks/Installation process.ipynb

@@ -1,5 +1,5 @@
 {
- "cells": [
+  "cells": [
   {
    "cell_type": "markdown",
    "metadata": {},

exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb

@@ -1,5 +1,5 @@
 {
- "cells": [
+  "cells": [
   {
    "cell_type": "markdown",
    "metadata": {},

exercizes/notebooks/ex_1_com_sin_track_talos.ipynb

@@ -1,5 +1,5 @@
 {
- "cells": [
+  "cells": [
   {
    "cell_type": "markdown",
    "metadata": {},

exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb

@@ -1,5 +1,5 @@
 {
- "cells": [
+  "cells": [
   {
    "cell_type": "code",
    "execution_count": null,

exercizes/tsid_biped.py

@@ -4,7 +4,6 @@
 
 import numpy as np
 import pinocchio as pin
-
 import tsid
 
 

exercizes/tsid_manipulator.py

@@ -6,7 +6,6 @@
 import numpy as np
 import numpy.matlib as matlib
 import pinocchio as se3
-
 import tsid