Skip to content

Commit 0a83575

Browse files
egordvegordv
authored
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
1 parent f938e35 commit 0a83575

35 files changed

+1489
-32
lines changed

‎CMakeLists.txt

+13-6
Original file line numberDiff line numberDiff line change
@@ -147,7 +147,8 @@ set(${PROJECT_NAME}_TASKS_HEADERS
147147
include/tsid/tasks/task-joint-posture.hpp
148148
include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
149149
include/tsid/tasks/task-capture-point-inequality.hpp
150-
include/tsid/tasks/task-angular-momentum-equality.hpp)
150+
include/tsid/tasks/task-angular-momentum-equality.hpp
151+
include/tsid/tasks/task-two-frames-equality.hpp)
151152

152153
set(${PROJECT_NAME}_CONTACTS_HEADERS
153154
include/tsid/contacts/fwd.hpp
@@ -156,7 +157,8 @@ set(${PROJECT_NAME}_CONTACTS_HEADERS
156157
include/tsid/contacts/contact-point.hpp
157158
include/tsid/contacts/measured-force-base.hpp
158159
include/tsid/contacts/measured-3Dforce.hpp
159-
include/tsid/contacts/measured-6Dwrench.hpp)
160+
include/tsid/contacts/measured-6Dwrench.hpp
161+
include/tsid/contacts/contact-two-frame-positions.hpp)
160162

161163
set(${PROJECT_NAME}_TRAJECTORIES_HEADERS
162164
include/tsid/trajectories/fwd.hpp
@@ -264,12 +266,17 @@ set(${PROJECT_NAME}_TASKS_SOURCES
264266
src/tasks/task-capture-point-inequality.cpp
265267
src/tasks/task-motion.cpp
266268
src/tasks/task-se3-equality.cpp
267-
src/tasks/task-angular-momentum-equality.cpp)
269+
src/tasks/task-angular-momentum-equality.cpp
270+
src/tasks/task-two-frames-equality.cpp)
268271

269272
set(${PROJECT_NAME}_CONTACTS_SOURCES
270-
src/contacts/contact-base.cpp src/contacts/contact-6d.cpp
271-
src/contacts/contact-point.cpp src/contacts/measured-force-base.cpp
272-
src/contacts/measured-3Dforce.cpp src/contacts/measured-6Dwrench.cpp)
273+
src/contacts/contact-base.cpp
274+
src/contacts/contact-6d.cpp
275+
src/contacts/contact-point.cpp
276+
src/contacts/measured-force-base.cpp
277+
src/contacts/measured-3Dforce.cpp
278+
src/contacts/measured-6Dwrench.cpp
279+
src/contacts/contact-two-frame-positions.cpp)
273280

274281
set(${PROJECT_NAME}_TRAJECTORIES_SOURCES
275282
src/trajectories/trajectory-se3.cpp

‎bindings/python/contacts/contact-two-frame-positions.cpp

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
//
2+
// Copyright (c) 2023 MIPT
3+
//
4+
// This file is part of tsid
5+
// tsid is free software: you can redistribute it
6+
// and/or modify it under the terms of the GNU Lesser General Public
7+
// License as published by the Free Software Foundation, either version
8+
// 3 of the License, or (at your option) any later version.
9+
// tsid is distributed in the hope that it will be
10+
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11+
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12+
// General Lesser Public License for more details. You should have
13+
// received a copy of the GNU Lesser General Public License along with
14+
// tsid If not, see
15+
// <http://www.gnu.org/licenses/>.
16+
//
17+
18+
#include "tsid/bindings/python/contacts/contact-two-frame-positions.hpp"
19+
#include "tsid/bindings/python/contacts/expose-contact.hpp"
20+
21+
namespace tsid {
22+
namespace python {
23+
void exposeContactTwoFramePositions() {
24+
ContactTwoFramePositionsPythonVisitor<
25+
tsid::contacts::ContactTwoFramePositions>::
26+
expose("ContactTwoFramePositions");
27+
}
28+
} // namespace python
29+
} // namespace tsid

‎bindings/python/tasks/task-two-frames-equality.cpp

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
//
2+
// Copyright (c) 2023 MIPT
3+
//
4+
// This file is part of tsid
5+
// tsid is free software: you can redistribute it
6+
// and/or modify it under the terms of the GNU Lesser General Public
7+
// License as published by the Free Software Foundation, either version
8+
// 3 of the License, or (at your option) any later version.
9+
// tsid is distributed in the hope that it will be
10+
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11+
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12+
// General Lesser Public License for more details. You should have
13+
// received a copy of the GNU Lesser General Public License along with
14+
// tsid If not, see
15+
// <http://www.gnu.org/licenses/>.
16+
//
17+
18+
#include "tsid/bindings/python/tasks/task-two-frames-equality.hpp"
19+
#include "tsid/bindings/python/tasks/expose-tasks.hpp"
20+
21+
namespace tsid {
22+
namespace python {
23+
void exposeTaskTwoFramesEquality() {
24+
TaskTwoFramesEqualityPythonVisitor<
25+
tsid::tasks::TaskTwoFramesEquality>::expose("TaskTwoFramesEquality");
26+
}
27+
} // namespace python
28+
} // namespace tsid

‎demo/demo_quadruped.py

+1-2
Original file line numberDiff line numberDiff line change
@@ -6,11 +6,10 @@
66
import gepetto.corbaserver
77
import numpy as np
88
import pinocchio as pin
9+
import tsid
910
from numpy import nan
1011
from numpy.linalg import norm as norm
1112

12-
import tsid
13-
1413
sys.path += [os.getcwd() + "/../exercizes"]
1514
import matplotlib.pyplot as plt
1615
import plot_utils as plut

‎demo/demo_tsid_talos_gripper_closed_kinematic_chain.py

+196
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,196 @@
1+
"""
2+
Simple demo of usage of ContactTwoFramePositions in TSID
3+
Make the Talos gripper model works with closed kinematic chains
4+
(c) MIPT
5+
"""
6+
import os
7+
import time
8+
9+
import numpy as np
10+
import pinocchio as pin
11+
import tsid
12+
from numpy import nan
13+
from numpy.linalg import norm as norm
14+
15+
np.set_printoptions(precision=3, linewidth=200, suppress=True)
16+
17+
LINE_WIDTH = 60
18+
print("".center(LINE_WIDTH, "#"))
19+
print(
20+
" Demo of TSID with Closed Kinematic Chains via ContactTwoFramePositions".center(
21+
LINE_WIDTH, "#"
22+
)
23+
)
24+
print("".center(LINE_WIDTH, "#"), "\n")
25+
26+
w_ee = 1.0 # weight of end effector task
27+
w_posture = 1e-3 # weight of joint posture task
28+
29+
kp_ee = 10.0 # proportional gain of end effector task
30+
kp_posture = 10.0 # proportional gain of joint posture task
31+
32+
dt = 0.001 # controller time step
33+
PRINT_N = 500 # print every PRINT_N time steps
34+
DISPLAY_N = 25 # update robot configuration in viwewer every DISPLAY_N time steps
35+
N_SIMULATION = 6000 # number of time steps simulated
36+
37+
# Loading Talos gripper model modified with extra links to mark the position of contact creation
38+
# Talos gripepr model (c) 2016, PAL Robotics, S.L.
39+
# Please use https://github.com/egordv/tsid_demo_closed_kinematic_chain repo for the model files
40+
41+
filename = str(os.path.dirname(os.path.abspath(__file__)))
42+
path = filename + "../../tsid_demo_closed_kinematic_chain/models/talos_gripper"
43+
urdf = path + "../../tsid_demo_closed_kinematic_chain/urdf/talos_gripper_half.urdf"
44+
vector = pin.StdVec_StdString()
45+
vector.extend(item for item in path)
46+
47+
robot = tsid.RobotWrapper(urdf, vector, False) # Load with fixed base
48+
49+
# for viewer
50+
robot_display = pin.RobotWrapper.BuildFromURDF(urdf, [path])
51+
robot_display.initViewer(loadModel=True)
52+
53+
model = robot.model()
54+
q = np.zeros(robot.nq)
55+
v = np.zeros(robot.nv)
56+
57+
viz = pin.visualize.MeshcatVisualizer(
58+
robot_display.model,
59+
robot_display.collision_model,
60+
robot_display.visual_model,
61+
)
62+
viz.initViewer(loadModel=True, open=True)
63+
64+
t = 0.0 # time
65+
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
66+
invdyn.computeProblemData(t, q, v)
67+
data = invdyn.data()
68+
69+
70+
fingertip_name = "gripper_left_fingertip_3_link"
71+
H_fingertip_ref = robot.framePosition(invdyn.data(), model.getFrameId(fingertip_name))
72+
73+
fingertipPositionTask = tsid.TaskSE3Equality(
74+
"task-fingertip-position", robot, fingertip_name
75+
)
76+
fingertipPositionTask.useLocalFrame(False)
77+
fingertipPositionTask.setKp(kp_ee * np.ones(6))
78+
fingertipPositionTask.setKd(2.0 * np.sqrt(kp_ee) * np.ones(6))
79+
trajFingertipPosition = tsid.TrajectorySE3Constant(
80+
"traj-fingertip-position", H_fingertip_ref
81+
)
82+
sampleFingertipPosition = trajFingertipPosition.computeNext()
83+
fingertipPositionTask.setReference(sampleFingertipPosition)
84+
invdyn.addMotionTask(fingertipPositionTask, w_ee, 1, 0.0)
85+
86+
postureTask = tsid.TaskJointPosture("task-posture", robot)
87+
postureTask.setKp(kp_posture * np.ones(robot.nv))
88+
postureTask.setKd(2.0 * np.sqrt(kp_posture) * np.ones(robot.nv))
89+
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
90+
91+
92+
# Creating a closed kinematic chain in TSID formulation by creating a contact between two frames, for which there are special links in URDF
93+
ContactTwoFramePositionsFingertipBottomAxis = tsid.ContactTwoFramePositions(
94+
"contact-two-frame-positions-fingertip-bottom-axis",
95+
robot,
96+
"gripper_left_motor_single_link_ckc_axis",
97+
"gripper_left_fingertip_3_link_ckc_axis",
98+
-1000,
99+
1000,
100+
)
101+
twoFramesContact_Kp = 300
102+
ContactTwoFramePositionsFingertipBottomAxis.setKp(twoFramesContact_Kp * np.ones(3))
103+
ContactTwoFramePositionsFingertipBottomAxis.setKd(
104+
2.0 * np.sqrt(twoFramesContact_Kp) * np.ones(3)
105+
)
106+
107+
twoFramesContact_w_forceRef = 1e-5
108+
invdyn.addRigidContact(
109+
ContactTwoFramePositionsFingertipBottomAxis, twoFramesContact_w_forceRef, 1.0, 1
110+
)
111+
112+
# Setting actuation to zero for passive joints in kinematic chain via TaskActuationBounds
113+
tau_max = model.effortLimit[-robot.na :]
114+
tau_max[
115+
0
116+
] = 0.0 # setting gripper_left_inner_single_joint to passive contstrainig it's actuation bounds to zero
117+
tau_max[
118+
1
119+
] = 0.0 # setting gripper_left_fingertip_3_joint to passive contstrainig it's actuation bounds to zero
120+
tau_min = -tau_max
121+
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
122+
actuationBoundsTask.setBounds(tau_min, tau_max)
123+
invdyn.addActuationTask(actuationBoundsTask, 1.0, 0, 0.0)
124+
125+
q_ref = q
126+
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
127+
128+
print(
129+
"Create QP solver with ",
130+
invdyn.nVar,
131+
" variables, ",
132+
invdyn.nEq,
133+
" equality and ",
134+
invdyn.nIn,
135+
" inequality constraints",
136+
)
137+
solver = tsid.SolverHQuadProgFast("qp solver")
138+
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
139+
140+
offset = sampleFingertipPosition.pos()
141+
offset[:3] += np.array([0, -0.04, 0])
142+
amp = np.array([0.0, 0.04, 0.0])
143+
two_pi_f = 2 * np.pi * np.array([0.0, 0.5, 0.0])
144+
two_pi_f_amp = np.multiply(two_pi_f, amp)
145+
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
146+
147+
pEE = offset.copy()
148+
vEE = np.zeros(6)
149+
aEE = np.zeros(6)
150+
151+
i = 0
152+
while True:
153+
time_start = time.time()
154+
155+
# Setting gripper finger task target to sine motion
156+
pEE[:3] = offset[:3] + amp * np.sin(two_pi_f * t)
157+
vEE[:3] = two_pi_f_amp * np.cos(two_pi_f * t)
158+
aEE[:3] = -two_pi_f_squared_amp * np.sin(two_pi_f * t)
159+
sampleFingertipPosition.value(pEE)
160+
sampleFingertipPosition.derivative(vEE)
161+
sampleFingertipPosition.second_derivative(aEE)
162+
163+
fingertipPositionTask.setReference(sampleFingertipPosition)
164+
165+
samplePosture = trajPosture.computeNext()
166+
postureTask.setReference(samplePosture)
167+
168+
# Computing HQP
169+
HQPData = invdyn.computeProblemData(t, q, v)
170+
if i == 0:
171+
HQPData.print_all()
172+
173+
sol = solver.solve(HQPData)
174+
if sol.status != 0:
175+
print("[%d] QP problem could not be solved! Error code:" % (i), sol.status)
176+
break
177+
178+
tau = invdyn.getActuatorForces(sol)
179+
dv = invdyn.getAccelerations(sol)
180+
181+
if i % PRINT_N == 0:
182+
print("Time %.3f" % (t))
183+
184+
v_mean = v + 0.5 * dt * dv
185+
v += dt * dv
186+
q = pin.integrate(robot.model(), q, dt * v_mean)
187+
t += dt
188+
189+
if i % DISPLAY_N == 0:
190+
viz.display(q)
191+
192+
time_spent = time.time() - time_start
193+
if time_spent < dt:
194+
time.sleep(dt - time_spent)
195+
196+
i = i + 1

‎exercizes/ex_0_ur5_joint_space_control.py

+1-2
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,11 @@
77
import numpy as np
88
import pinocchio as pin
99
import plot_utils as plut
10+
import tsid
1011
import ur5_conf as conf
1112
from numpy import nan
1213
from numpy.linalg import norm as norm
1314

14-
import tsid
15-
1615
print("".center(conf.LINE_WIDTH, "#"))
1716
print(" Joint Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, "#"))
1817
print("".center(conf.LINE_WIDTH, "#"), "\n")

‎exercizes/ex_4_walking.py

+1-2
Original file line numberDiff line numberDiff line change
@@ -4,12 +4,11 @@
44
import matplotlib.pyplot as plt
55
import numpy as np
66
import plot_utils as plut
7+
import tsid
78
from numpy import nan
89
from numpy.linalg import norm as norm
910
from tsid_biped import TsidBiped
1011

11-
import tsid
12-
1312
print("".center(conf.LINE_WIDTH, "#"))
1413
print(" Test Walking ".center(conf.LINE_WIDTH, "#"))
1514
print("".center(conf.LINE_WIDTH, "#"), "\n")

‎exercizes/notebooks/Installation process.ipynb

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
{
2-
"cells": [
2+
"cells": [
33
{
44
"cell_type": "markdown",
55
"metadata": {},

‎exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
{
2-
"cells": [
2+
"cells": [
33
{
44
"cell_type": "markdown",
55
"metadata": {},

‎exercizes/notebooks/ex_1_com_sin_track_talos.ipynb

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
{
2-
"cells": [
2+
"cells": [
33
{
44
"cell_type": "markdown",
55
"metadata": {},

‎exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
{
2-
"cells": [
2+
"cells": [
33
{
44
"cell_type": "code",
55
"execution_count": null,

‎exercizes/tsid_biped.py

-1
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,6 @@
44

55
import numpy as np
66
import pinocchio as pin
7-
87
import tsid
98

109

‎exercizes/tsid_manipulator.py

-1
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@
66
import numpy as np
77
import numpy.matlib as matlib
88
import pinocchio as se3
9-
109
import tsid
1110

1211

0 commit comments

Comments
 (0)