Commit c8786cea authored by Pierre Desreumaux's avatar Pierre Desreumaux
Browse files

Merge branch 'devel' of https://github.com/stack-of-tasks/tsid into capture-point

merge devel to capture-point for pull request
parents 9a30e7b3 84c0158e
[submodule "cmake"]
path = cmake
url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
[submodule ".travis"]
path = .travis
url = https://github.com/jrl-umi3218/jrl-travis.git
......@@ -70,7 +70,10 @@ IF(EIGEN_NO_AUTOMATIC_RESIZING)
ENDIF(EIGEN_NO_AUTOMATIC_RESIZING)
# Project dependencies
ADD_PROJECT_DEPENDENCY(pinocchio 2.3.1 REQUIRED PKG_CONFIG_REQUIRES "pinocchio >= 2.3.1")
ADD_PROJECT_DEPENDENCY(pinocchio 2.3.1 REQUIRED
PKG_CONFIG_REQUIRES "pinocchio >= 2.3.1")
ADD_PROJECT_DEPENDENCY(eiquadprog 1.1.3 REQUIRED
PKG_CONFIG_REQUIRES "eiquadprog >= 1.1.3")
SET(BOOST_COMPONENTS filesystem system unit_test_framework)
......@@ -127,10 +130,6 @@ SET(${PROJECT_NAME}_TRAJECTORIES_HEADERS
)
SET(${PROJECT_NAME}_SOLVERS_HEADERS
include/tsid/solvers/eiquadprog_2011.hpp
include/tsid/solvers/eiquadprog-rt.hpp
include/tsid/solvers/eiquadprog-rt.hxx
include/tsid/solvers/eiquadprog-fast.hpp
include/tsid/solvers/fwd.hpp
include/tsid/solvers/utils.hpp
include/tsid/solvers/solver-HQP-output.hpp
......@@ -219,7 +218,6 @@ SET(${PROJECT_NAME}_TRAJECTORIES_SOURCES
)
SET(${PROJECT_NAME}_SOLVERS_SOURCES
src/solvers/eiquadprog-fast.cpp
src/solvers/solver-HQP-base.cpp
src/solvers/solver-HQP-factory.cpp
src/solvers/solver-HQP-eiquadprog.cpp
......@@ -256,7 +254,7 @@ ADD_LIBRARY(${PROJECT_NAME} SHARED
${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES}
pinocchio::pinocchio)
pinocchio::pinocchio eiquadprog::eiquadprog)
IF(SUFFIX_SO_VERSION)
......
BSD 2-Clause License
Copyright (c) 2019, CNRS, Univ. of Trento, INRIA, Max Planck Institure
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
# TSID - Task Space Inverse Dynamics
[![Building Status](https://travis-ci.org/stack-of-tasks/tsid.svg?branch=master)](https://travis-ci.org/stack-of-tasks/tsid)
[![Pipeline status](https://gepgitlab.laas.fr/stack-of-tasks/tsid/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/stack-of-tasks/tsid/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/stack-of-tasks/tsid/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/stack-of-tasks/tsid/master/coverage/)
[![Pipeline status](https://gitlab.laas.fr/stack-of-tasks/tsid/badges/master/pipeline.svg)](https://gitlab.laas.fr/stack-of-tasks/tsid/commits/master)
[![Coverage report](https://gitlab.laas.fr/stack-of-tasks/tsid/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/stack-of-tasks/tsid/master/coverage/)
TSID is C++ library for optimization-based inverse-dynamics control based on the rigid multi-body dynamics library [Pinocchio](https://github.com/stack-of-tasks/pinocchio).
Take a look at the project [wiki](https://github.com/stack-of-tasks/tsid/wiki) for more details.
......@@ -11,6 +11,7 @@ Take a look at the project [wiki](https://github.com/stack-of-tasks/tsid/wiki) f
* boost (unit_test_framework)
* eigen3
* [pinocchio](https://github.com/stack-of-tasks/pinocchio)
* [eiquadprog](https://github.com/stack-of-tasks/eiquadprog)
To install eigen3 on Ubuntu you can use apt-get:
`sudo apt-get install libeigen3-dev`
......@@ -33,15 +34,15 @@ To use this library in python, we offer python bindings based on Boost.Python an
To install EigenPy you can compile the source code:
git clone https://github.com/stack-of-tasks/eigenpy
or, on Ubuntu, you can use apt-get:
sudo apt-get install robotpkg-py27-eigenpy
For testing the python bindings, you can run the unit test scripts in the `script` folder, for instance:
ipython script/test_formulation.py
To run the demo using gepetto-viewer:
ipython demo/demo_romeo.py
......
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
Subproject commit 0ec0f542c02839bcbdf1faa8f5a4559dcf8632ec
import numpy as np
import numpy.matlib as matlib
from numpy import nan
from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
import pinocchio as se3
import pinocchio as pin
import tsid
import gepetto.corbaserver
import subprocess
......@@ -23,7 +22,7 @@ PLOT_JOINT_ACC = 1
PLOT_TORQUES = 0
USE_VIEWER = 1
vector = se3.StdVec_StdString()
vector = pin.StdVec_StdString()
vector.extend(item for item in conf.path)
robot = tsid.RobotWrapper(conf.urdf, vector, False)
model = robot.model()
......@@ -51,7 +50,7 @@ solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
if(USE_VIEWER):
robot_display = se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ])
robot_display = pin.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ])
l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
......@@ -88,9 +87,9 @@ for i in range(0, N):
time_start = time.time()
# set reference trajectory
q_ref[:,i] = q0 + np.multiply(amp, np.sin(two_pi_f*t + phi))
v_ref[:,i] = np.multiply(two_pi_f_amp, np.cos(two_pi_f*t + phi))
dv_ref[:,i] = np.multiply(two_pi_f_squared_amp, -np.sin(two_pi_f*t + phi))
q_ref[:,i] = q0 + amp * np.sin(two_pi_f*t + phi)
v_ref[:,i] = two_pi_f_amp * np.cos(two_pi_f*t + phi)
dv_ref[:,i] = -two_pi_f_squared_amp * np.sin(two_pi_f*t + phi)
samplePosture.pos(q_ref[:,i])
samplePosture.vel(v_ref[:,i])
samplePosture.acc(dv_ref[:,i])
......@@ -113,7 +112,7 @@ for i in range(0, N):
# numerical integration
v_mean = v[:,i] + 0.5*dt*dv[:,i]
v[:,i+1] = v[:,i] + dt*dv[:,i]
q[:,i+1] = se3.integrate(model, q[:,i], dt*v_mean)
q[:,i+1] = pin.integrate(model, q[:,i], dt*v_mean)
t += conf.dt
if i%conf.DISPLAY_N == 0:
......
import numpy as np
import numpy.matlib as matlib
from numpy import nan
from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
......@@ -9,7 +8,7 @@ import romeo_conf as conf
from tsid_biped import TsidBiped
print("".center(conf.LINE_WIDTH,'#'))
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print(" Test Task Space Inverse Dynamics - Biped ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
tsid = TsidBiped(conf)
......
......@@ -10,7 +10,7 @@ from tsid_manipulator import TsidManipulator
import ur5_reaching_conf as conf
print(("".center(conf.LINE_WIDTH,'#')))
print((" Task Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#')))
print((" TSID - Manipulator End-Effector Sin Tracking ".center(conf.LINE_WIDTH, '#')))
print(("".center(conf.LINE_WIDTH,'#'), '\n'))
PLOT_EE_POS = 1
......
......@@ -8,7 +8,7 @@ import romeo_conf as conf
from tsid_biped import TsidBiped
print("".center(conf.LINE_WIDTH,'#'))
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print(" TSID - Biped Sin Tracking ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
tsid = TsidBiped(conf)
......
......@@ -153,7 +153,7 @@ def run_simu():
J = np.vstack((J_LF, J_RF))
J_com = tsid.comTask.compute(t, q, v, data).matrix
A = np.vstack((J_com, J))
b = np.vstack((np.array(push_robot_com_vel).T, np.zeros((J.shape[0],1))))
b = np.concatenate((np.array(push_robot_com_vel), np.zeros(J.shape[0])))
v = np.linalg.lstsq(A, b, rcond=-1)[0]
if i%conf.DISPLAY_N == 0:
......
......@@ -20,14 +20,15 @@ filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
srdf = path + '/srdf/romeo_collision.srdf'
lxp = 0.10 # foot length in positive x direction
lxn = 0.05 # foot length in negative x direction
lyp = 0.05 # foot length in positive y direction
lyn = 0.05 # foot length in negative y direction
foot_scaling = 1.
lxp = foot_scaling*0.10 # foot length in positive x direction
lxn = foot_scaling*0.05 # foot length in negative x direction
lyp = foot_scaling*0.05 # foot length in positive y direction
lyn = foot_scaling*0.05 # foot length in negative y direction
lz = 0.07 # foot sole height with respect to ankle joint
mu = 0.3 # friction coefficient
fMin = 5.0 # minimum normal force
fMax = 1000.0 # maximum normal force
fMin = 0.0 # minimum normal force
fMax = 1e6 # maximum normal force
rf_frame_name = "RAnkleRoll" # right foot frame name
lf_frame_name = "LAnkleRoll" # left foot frame name
contactNormal = np.matrix([0., 0., 1.]).T # direction of the normal to the contact surface
......@@ -42,18 +43,19 @@ g = 9.81 # norm of the gravity vector
foot_step_0 = np.array([0.0, -0.096]) # initial foot step position in x-y
dt_mpc = 0.1 # sampling time interval
T_step = 1.2 # time needed for every step
step_length = 0.1 # fixed step length
step_length = 0.05 # fixed step length
step_height = 0.05 # fixed step height
nb_steps = 4 # number of desired walking steps
# configuration for TSID
# ----------------------------------------------
dt = 0.002 # controller time step
T_pre = 1.0 # simulation time before starting to walk
T_post = 2.0 # simulation time after walking
T_pre = 1.5 # simulation time before starting to walk
T_post = 1.5 # simulation time after walking
w_com = 1.0 # weight of center of mass task
w_foot = 1e0 # weight of the foot motion task
w_contact = 1e2 # weight of the foot in contact
w_posture = 1e-4 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
w_torque_bounds = 0.0 # weight of the torque bounds
......
......@@ -15,8 +15,8 @@ print("".center(conf.LINE_WIDTH,'#'), '\n')
PLOT_COM = 1
PLOT_COP = 1
PLOT_FOOT_TRAJ = 0
PLOT_TORQUES = 1
PLOT_JOINT_VEL = 1
PLOT_TORQUES = 0
PLOT_JOINT_VEL = 0
data = np.load(conf.DATA_FILE_TSID)
......@@ -61,7 +61,7 @@ com_acc_des = np.empty((3, N+N_post))*nan # acc_des = acc_ref - Kp*pos_err - Kd*
x_rf = tsid.get_placement_RF().translation
offset = x_rf - x_RF_ref[:,0]
for i in range(N):
com_pos_ref[:,i] += offset
com_pos_ref[:,i] += offset + np.array([0.,0.,0.0])
x_RF_ref[:,i] += offset
x_LF_ref[:,i] += offset
......
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.