Commit ff47d843 authored by Guilhem Saurel's avatar Guilhem Saurel

merge devel into cmake-export

parents 2aad7369 9ecc8ccd
# Copyright 2016, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
#
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
SET(PROJECT_NAMESPACE stack-of-tasks)
SET(PROJECT_NAME sot-talos)
SET(PROJECT_DESCRIPTION "dynamic-graph package for Talos robot")
SET(PROJECT_URL "http://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/ros.cmake)
INCLUDE(cmake/python.cmake)
OPTION(SUFFIX_SO_VERSION
"Suffix shared library name by a string depending on git status of project"
ON)
# Export CMake Target
SET(PROJECT_USE_CMAKE_EXPORT TRUE)
......@@ -43,6 +54,8 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
plugindir
)
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
# Search for dependencies.
......@@ -63,6 +76,9 @@ ADD_OPTIONAL_DEPENDENCY("pyrene-motions")
# Search for dependencies.
# Boost
FINDPYTHON()
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_PATH})
# Handle rpath necessary to handle ROS multiplace packages
# libraries inclusion
SET(CMAKE_INSTALL_LIBDIR lib)
......@@ -129,14 +145,3 @@ IF(TALOS_DATA_FOUND)
)
ENDIF(TALOS_DATA_FOUND)
SETUP_PROJECT_PACKAGE_FINALIZE()
get_cmake_property(_variableNames VARIABLES)
list (SORT _variableNames)
foreach (_variableName ${_variableNames})
message(STATUS "${_variableName}=${${_variableName}}")
endforeach()
LOGGING_FINALIZE()
Subproject commit 7eca9ee6c9d1c4ee20eb82272e94f9d11642053a
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
/* Copyright 2018, CNRS
* Author: O. Stasse
*
*
BSD 2-Clause License
Copyright (c) 2017, Stack Of Tasks development team
......@@ -33,16 +33,17 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
\mainpage
\section sec_intro Introduction
This library implements a class called Device for the TALOS PAL-Robotics humanoid robots to be controlled with the Stack-Of-Tasks.
A derived class is used for the first prototype of this line of humanoid robot, Pyrene.
This library implements a class called Device for the TALOS PAL-Robotics humanoid robots to be controlled with the
Stack-Of-Tasks. A derived class is used for the first prototype of this line of humanoid robot, Pyrene.
This class is implementing a perception-action loop.
To achieve this goal it is providing a Hardware Abstract Layer to communicate with the SoT control infra-structure and the robot or a simulator.
It is fully compatible with the roscontrol_sot package to run on a TALOS humanoid robot or with the Gazebo simulator.
To achieve this goal it is providing a Hardware Abstract Layer to communicate with the SoT control infra-structure and
the robot or a simulator. It is fully compatible with the roscontrol_sot package to run on a TALOS humanoid robot or
with the Gazebo simulator.
The sot-talos package contains also the class sot-talos-controller. This class is used to start the multithreading environment which is handling ROS request, starts
the python interpreter to control the SoT and finally handle
the control states: initialization, nominal usage, stopping or cancellation.
The sot-talos package contains also the class sot-talos-controller. This class is used to start the multithreading
environment which is handling ROS request, starts the python interpreter to control the SoT and finally handle the
control states: initialization, nominal usage, stopping or cancellation.
It also provides python scripts to be used to interact with the robot.
......@@ -57,6 +58,6 @@ It also provides python scripts to be used to interact with the robot.
<li> motor_angles </li>
<li> p_gains </li>
<li> d_gains</li>
\section sot-talos-controller
\section sot-talos-controller
*/
# Copyright 2016, R. Budhiraja, Olivier Stasse, CNRS
#
# This file is part of sot-talos.
# sot-talos 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.
#
# sot-talos 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
# sot-talos. If not, see <http://www.gnu.org/licenses/>.
INCLUDE(../cmake/python.cmake)
FINDPYTHON()
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
ADD_LIBRARY(${NAME} SHARED ${SOURCES})
SET_TARGET_PROPERTIES(${lib} PROPERTIES
PREFIX ""
SOVERSION ${PROJECT_VERSION})
)
IF (SUFFIX_SO_VERSION)
SET_TARGET_PROPERTIES(${lib} PROPERTIES
SOVERSION ${PROJECT_VERSION}
)
ENDIF()
TARGET_LINK_LIBRARIES(${NAME} dynamic-graph::dynamic-graph)
TARGET_LINK_LIBRARIES(${NAME} sot-core::sot-core)
TARGET_LINK_LIBRARIES(${NAME} eigen3)
TARGET_LINK_LIBRARIES(${NAME} roscpp)
TARGET_LINK_LIBRARIES(${NAME} roscpp)
TARGET_LINK_LIBRARIES(${NAME} roscpp)
PKG_CONFIG_USE_DEPENDENCY(${NAME} pinocchio)
INSTALL(TARGETS ${NAME}
......@@ -54,7 +42,7 @@ PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "talos.py" )
SET(FILES __init__.py robot.py)
# Install dynamic_graph.sot.pyrene
# Install dynamic_graph.sot.pyrene
SET(PYTHON_MODULE dynamic_graph/sot/pyrene)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
......@@ -124,7 +112,11 @@ MACRO(build_talos_controller)
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} sot-talos-device)
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME}
dynamic_graph_bridge::ros_interpreter)
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic_graph_bridge")
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} "ros_bridge")
IF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${Boost_LIBRARIES})
ENDIF(UNIX AND NOT APPLE)
......@@ -136,4 +128,3 @@ MACRO(build_talos_controller)
ENDMACRO()
build_talos_controller()
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph 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.
#
# dynamic-graph 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
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
print("Prologue Pyrene TALOS Robot")
# sys.argv is not defined when running the remove interpreter, but it is
# required by rospy
import sys
from dynamic_graph.entity import PyEntityFactoryClass
from dynamic_graph.sot.pyrene.robot import Robot
# Create the device.
# This entity behaves exactly like robotsimu except:
# 1. it does not provide the increment command
# 2. it forwards the robot control to the sot-abstract
# controller.
def makeRobot ():
if not hasattr(sys, 'argv'):
sys.argv = [
"dynamic_graph",
]
print("Prologue Pyrene TALOS Robot")
def makeRobot():
"""
Create the device.
This entity behaves exactly like robotsimu except:
1. it does not provide the increment command
2. it forwards the robot control to the sot-abstract
controller.
"""
DeviceTalos = PyEntityFactoryClass('DeviceTalos')
# Create the robot using the device.
robot = Robot(name = 'robot', device = DeviceTalos('PYRENE'))
robot.dynamic.com.recompute (0)
robot = Robot(name='robot', device=DeviceTalos('PYRENE'), fromRosParam=True)
robot.dynamic.com.recompute(0)
_com = robot.dynamic.com.value
robot.device.zmp.value = (_com[0], _com[1], 0.)
return robot
####################################
# --- IMPORTANT --- #
# #
......
# -*- coding: utf-8 -*-
# Copyright 2016, Olivier Stasse, CNRS
#
# This file is part of sot-talos.
# sot-talos 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.
#
# sot-talos 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
# sot-talos. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph.sot.talos import Talos
import numpy as np
class Robot (Talos):
class Robot(Talos):
"""
This class instantiates LAAS TALOS Robot
"""
halfSitting = (
0.0,
0.0,
1.018213,
0.00,
0.0,
0.0, # Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Left Leg
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Right Leg
0.0,
0.006761, # Chest
0.25847,
0.173046,
-0.0002,
-0.525366,
0.0,
-0.0,
0.1,
-0.005, # Left Arm
-0.25847,
-0.173046,
0.0002,
-0.525366,
0.0,
0.0,
0.1,
-0.005, # Right Arm
0.,
0. # Head
)
halfSitting = (0.0, 0.0, 1.018213, 0.00 , 0.0, 0.0, #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
0.0 , 0.006761, #Chest
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, -0.005, #Left Arm
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005, #Right Arm
0., 0. #Head
)
def __init__(self, name,
device = None,
tracer = None):
Talos.__init__(self,name,self.halfSitting,device,tracer)
def __init__(self, name, device=None, tracer=None, fromRosParam=None):
Talos.__init__(self, name, self.halfSitting, device, tracer)
"""
TODO:Confirm these values
# Define camera positions w.r.t gaze.
......@@ -64,7 +79,7 @@ class Robot (Talos):
(-0.03999, 0.00000, 0.99920, 0.145 - 0.03),
(0., 0., 0., 1.),
))
# Frames re-orientation:
# Frames re-orientation:
# Z = depth (increase from near to far)
# X = increase from left to right
# Y = increase from top to bottom
......@@ -74,7 +89,7 @@ class Robot (Talos):
[-1., 0., 0., 0.],
[ 0., -1., 0., 0.],
[ 0., 0., 0., 1.]])
for camera in [cameraBottomLeftPosition, cameraBottomRightPosition,
cameraTopLeftPosition, cameraTopRightPosition]:
camera[:] = camera * c1_M_c
......@@ -93,4 +108,5 @@ class Robot (Talos):
matrixToTuple(cameraTopRightPosition), "gaze"))
"""
__all__ = ["Robot"]
# -*- coding: utf-8 -*-
# Copyright 2012, CNRS-LAAS, Florent Lamiraux
#
# This file is part of sot-hrp2.
# sot-hrp2 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.
#
# sot-hrp2 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
# sot-hrp2. If not, see <http://www.gnu.org/licenses/>.
import numpy as np
from math import sqrt
from dynamic_graph.sot.pyrene.robot import Robot
from dynamic_graph.sot.core import RPYToMatrix
from dynamic_graph.sot.tools.se3 import SE3, SO3, R3
from dynamic_graph.sot.pyrene.robot import Robot
from dynamic_graph.sot.tools.se3 import R3, SE3
robot = Robot ('seqplay')
rpy2matrix = RPYToMatrix ('rpy2matrix')
robot = Robot('seqplay')
rpy2matrix = RPYToMatrix('rpy2matrix')
m = 56.868
g = 9.81
pos = None
zmp = None
hip = None
def convert (filename) :
def convert(filename):
"""
Convert a seqplay file in OpenHRP format to sot-tool format
"""
global pos, zmp, hip
openhrpPos = np.genfromtxt (filename + '.pos')
openhrpZmp = np.genfromtxt (filename + '.zmp')
nbConfig = len (openhrpPos)
if len (openhrpZmp) != nbConfig :
raise RuntimeError (filename + ".pos and " + filename +
".zmp have different lengths.")
openhrpPos = np.genfromtxt(filename + '.pos')
openhrpZmp = np.genfromtxt(filename + '.zmp')
nbConfig = len(openhrpPos)
if len(openhrpZmp) != nbConfig:
raise RuntimeError(filename + ".pos and " + filename + ".zmp have different lengths.")
try:
openhrpHip = np.genfromtxt (filename + '.hip')
openhrpHip = np.genfromtxt(filename + '.hip')
except IOError:
hip = []
for i in range (len (openhrpPos)):
hip.append ((openhrpPos [i][0], 0, 0, 0,))
openhrpHip = np.array (hip)
if len (openhrpHip) != nbConfig :
raise RuntimeError (filename + ".pos and " + filename +
".hip have different lengths.")
for i in range(len(openhrpPos)):
hip.append((
openhrpPos[i][0],
0,
0,
0,
))
openhrpHip = np.array(hip)
if len(openhrpHip) != nbConfig:
raise RuntimeError(filename + ".pos and " + filename + ".hip have different lengths.")
t = 1
featurePos = []
......@@ -62,110 +53,122 @@ def convert (filename) :
fixedFoot = None
fixedLeftFoot = None
fixedRightFoot = None
for (pos, zmp, hip) in zip (openhrpPos, openhrpZmp, openhrpHip) :
translation = 3*(0.,)
config = list (translation + tuple (hip [1:]) + tuple (pos [1:31]))
robot.dynamic.position.value = tuple (config)
for (pos, zmp, hip) in zip(openhrpPos, openhrpZmp, openhrpHip):
translation = 3 * (0., )
config = list(translation + tuple(hip[1:]) + tuple(pos[1:31]))
robot.dynamic.position.value = tuple(config)
robot.dynamic.position.time = t
robot.com.recompute (t)
robot.leftAnkle.position.recompute (t)
robot.rightAnkle.position.recompute (t)
lf = SE3 (robot.leftAnkle.position.value) * R3 (0., 0., -0.107)
rf = SE3 (robot.rightAnkle.position.value) * R3 (0., 0., -0.107)
robot.com.recompute(t)
robot.leftAnkle.position.recompute(t)
robot.rightAnkle.position.recompute(t)
lf = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
rf = SE3(robot.rightAnkle.position.value) * R3(0., 0., -0.107)
# find support foot
rpy2matrix.sin.value = tuple (hip [1:])
rpy2matrix.sout.recompute (t)
waist = SE3 (rpy2matrix.sout.value, translation)
zmpGlob = waist * R3 (tuple (zmp [1:]))
rpy2matrix.sin.value = tuple(hip[1:])
rpy2matrix.sout.recompute(t)
waist = SE3(rpy2matrix.sout.value, translation)
zmpGlob = waist * R3(tuple(zmp[1:]))
# fr = m g * (zmpGlob - lf | rf - lf)/||rf - lf||^2
# fl = (m g - fr)
fr = m * g * ((zmpGlob - lf)*(rf - lf)/((rf - lf)*(rf - lf)))
fr = m * g * ((zmpGlob - lf) * (rf - lf) / ((rf - lf) * (rf - lf)))
fl = m * g - fr
if (lf - zmpGlob) * (lf - zmpGlob) < (rf - zmpGlob) * (rf - zmpGlob) :
if (lf - zmpGlob) * (lf - zmpGlob) < (rf - zmpGlob) * (rf - zmpGlob):
supportFoot = lf
fixedFoot = fixedLeftFoot
else :
else:
supportFoot = rf
fixedFoot = fixedRightFoot
t+=1
t += 1
# move support foot to previous value
if fixedFoot is None:
config [2] -= supportFoot [2]
config[2] -= supportFoot[2]
else:
config [0] += fixedFoot [0] - supportFoot [0]
config [1] += fixedFoot [1] - supportFoot [1]
config [2] += fixedFoot [2] - supportFoot [2]
config[0] += fixedFoot[0] - supportFoot[0]
config[1] += fixedFoot[1] - supportFoot[1]
config[2] += fixedFoot[2] - supportFoot[2]
robot.dynamic.position.value = tuple (config)
robot.dynamic.position.value = tuple(config)
robot.dynamic.position.time = t
robot.com.recompute (t)
robot.leftAnkle.position.recompute (t)
robot.rightAnkle.position.recompute (t)
featurePos.append (config)
featureCom.append (robot.com.value)
featureLa.append (robot.leftAnkle.position.value)
featureRa.append (robot.rightAnkle.position.value)
forceLeftFoot.append ((0.,0.,fl,0.,0.,0.,))
forceRightFoot.append ((0.,0.,fr,0.,0.,0.,))
robot.com.recompute(t)
robot.leftAnkle.position.recompute(t)
robot.rightAnkle.position.recompute(t)
featurePos.append(config)
featureCom.append(robot.com.value)
featureLa.append(robot.leftAnkle.position.value)
featureRa.append(robot.rightAnkle.position.value)
forceLeftFoot.append((
0.,
0.,
fl,
0.,
0.,
0.,
))
forceRightFoot.append((
0.,
0.,
fr,
0.,
0.,
0.,
))
t += 1
fixedLeftFoot = \
SE3 (robot.leftAnkle.position.value) * R3 (0., 0., -0.107)
fixedRightFoot = \
SE3 (robot.rightAnkle.position.value) * R3 (0., 0., -0.107)
filePos = open (filename + '.posture', 'w')
fileLa = open (filename + '.la', 'w')
fileRa = open (filename + '.ra', 'w')
fileCom = open (filename + '.com', 'w')
fileFl = open (filename + '.fl', 'w')
fileFr = open (filename + '.fr', 'w')
fixedLeftFoot = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
fixedRightFoot = SE3(robot.rightAnkle.position.value) * R3(0., 0., -0.107)
filePos = open(filename + '.posture', 'w')
fileLa = open(filename + '.la', 'w')
fileRa = open(filename + '.ra', 'w')
fileCom = open(filename + '.com', 'w')
fileFl = open(filename + '.fl', 'w')
fileFr = open(filename + '.fr', 'w')
dt = .005
for (pos, la, ra, com,
force_lf, force_rf, i) in zip (featurePos, featureLa, featureRa,
featureCom, forceLeftFoot,
forceRightFoot, xrange (10000000)) :
t = i*dt
filePos.write ("{0}".format (t))
fileLa.write ("{0}".format (t))
fileRa.write ("{0}".format (t))
fileCom.write ("{0}".format (t))
fileFl.write ("{0}".format (t))
fileFr.write ("{0}".format (t))
for (pos, la, ra, com, force_lf, force_rf, i) in zip(featurePos, featureLa, featureRa, featureCom, forceLeftFoot,
forceRightFoot, range(10000000)):
t = i * dt
filePos.write("{0}".format(t))
fileLa.write("{0}".format(t))
fileRa.write("{0}".format(t))
fileCom.write("{0}".format(t))
fileFl.write("{0}".format(t))
fileFr.write("{0}".format(t))
for x in pos:
filePos.write ("\t{0}".format (x))
filePos.write("\t{0}".format(x))
for row in la:
for x in row:
fileLa.write ("\t{0}".format (x))
fileLa.write("\t{0}".format(x))
for row in ra:
for x in row:
fileRa.write ("\t{0}".format (x))
fileRa.write("\t{0}".format(x))
for x in com:
fileCom.write ("\t{0}".format (x))
fileCom.write("\t{0}".format(x))
for x in force_lf:
fileFl.write ("\t{0}".format (x))
fileFl.write("\t{0}".format(x))
for x in force_rf:
fileFr.write ("\t{0}".format (x))
filePos.write ("\n")
fileLa.write ("\n")
fileRa.write ("\n")
fileCom.write ("\n")
fileFl.write ("\n")
fileFr.write ("\n")
filePos.close ()
fileLa.close ()
fileRa.close ()
fileCom.close ()
fileFl.close ()
fileFr.close ()
fileFr.write("\t{0}".format(x))
filePos.write("\n")
fileLa.write("\n")
fileRa.write("\n")
fileCom.write("\n")
fileFl.write("\n")
fileFr.write("\n")
filePos.close()
fileLa.close()
fileRa.close()
fileCom.close()
fileFl.close()
fileFr.close()
if __name__ == '__main__':
#convert ('/opt/grx3.0/HRP2LAAS/etc/walkfwd')
pass
# convert('/opt/grx3.0/HRP2LAAS/etc/walkfwd')