Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/sot-dyninv
1 result
Show changes
Commits on Source (63)
Hide whitespace changes
Inline Side-by-side
Showing
with 450 additions and 538 deletions
.gitignore
View file @ 60cf95b2
*build
*build*
*~
*.pyc
*/#*#
......
.gitlab-ci.yml 0 → 100644
View file @ 60cf95b2
include: http://rainboard.laas.fr/project/sot-dyninv/.gitlab-ci.yml
.travis.yml 0 → 100644
View file @ 60cf95b2
matrix:
allow_failures:
- compiler: clang
script: ./.travis/build
language: cpp
after_success:
- coveralls -e _travis/install -e tests
- git config --global user.name "Travis CI"
- git config --global user.email "thomas.moulard+travis@gmail.com"
- git remote set-url origin https://thomas-moulard:${GH_TOKEN}@github.com/stack-of-tasks/dynamic-graph.git
- git fetch origin gh-pages:gh-pages
- cd _travis/build/doc && ../../../cmake/github/update-doxygen-doc.sh
branches:
only:
- master
notifications:
email:
- hpp-source@laas.fr
before_install:
- git submodule update --init --recursive
- sudo apt-get update -qq
- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev python-sphinx
- sudo pip install cpp-coveralls --use-mirrors
env:
global:
secure: kxgFbSRYzVa2zL4u3V+53D/cte66pnqWriB/em9Jslaa7EGu9xEV57nXzsSRJe29xdSipAY56RgehjoMo36DtpTO/5zj2aeIscYQac47fTvZmgI0UazyMnhDR1uB8UIy01U4NdXyGgLKNW1kWA48ulw+e7+FpOpnlAztyNZBIwk=
compiler:
- clang
- gcc
.travis/build 0 → 100755
View file @ 60cf95b2
#!/bin/sh
set -ev
# Directories.
root_dir=`pwd`
build_dir="$root_dir/_travis/build"
install_dir="$root_dir/_travis/install"
# Shortcuts.
git_clone="git clone --quiet --recursive"
# Create layout.
rm -rf "$build_dir" "$install_dir"
mkdir -p "$build_dir"
mkdir -p "$install_dir"
# Setup environment variables.
export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH"
export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH"
export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH"
install_dependency()
{
echo "--> Compiling $1"
mkdir -p "$build_dir/$1"
cd "$build_dir"
$git_clone "git://github.com/$1" "$1"
cd "$build_dir/$1"
cmake . -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
make install
}
# Retrieve jrl-mathtools
install_dependency jrl-umi3218/jrl-mathtools
install_dependency jrl-umi3218/jrl-mal
install_dependency stack-of-tasks/dynamic-graph
install_dependency stack-of-tasks/dynamic-graph-python
install_dependency stack-of-tasks/sot-core
install_dependency stack-of-tasks/soth
# Compile and run tests
cd "$build_dir"
cmake "$root_dir" -DCMAKE_INSTALL_PREFIX="$install_dir" \
-DCMAKE_CXX_FLAGS="--coverage" \
-DCMAKE_EXE_LINKER_FLAGS="--coverage" \
-DCMAKE_MODULE_LINKER_FLAGS="--coverage" \
-DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
make
make test
make install
AUTHORS
View file @ 60cf95b2
......@@ -3,3 +3,4 @@ This package was written by and with the assistance of:
* Nicolas Mansard nicolas.mansard@laas.fr
* Layale Saab lsaab@laas.fr
* Olivier Stasse olivier.stasse@aist.go.jp
* Oscar E. Ramos P. oramos@laas.fr
\ No newline at end of file
CMakeLists.txt
View file @ 60cf95b2
......@@ -17,14 +17,18 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/cpack.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAMESPACE stack-of-tasks)
SET(PROJECT_NAME sot-dyninv)
SET(PROJECT_DESCRIPTION "control by inverse dynamics.")
SET(PROJECT_URL "http://github.com/laas/sot-dyninv")
SET(PROJECT_URL "http://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
SET(DOXYGEN_USE_MATHJAX YES)
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
......@@ -35,14 +39,16 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
plugindir
)
OPTION(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
OPTION(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF)
SETUP_PROJECT()
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.8.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 1.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 1.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("soth >= 0.0.1")
SEARCH_FOR_EIGEN()
# List plug-ins that will be compiled.
SET(libs
......@@ -56,6 +62,7 @@ SET(libs
task-dyn-joint-limits
task-dyn-limits
task-dyn-inequality
task-dyn-passing-point
solver-op-space
solver-dyn-reduced
......@@ -80,7 +87,6 @@ SET(headers
pseudo-robot-dynamic.h
robot-dyn-simu.h
mal-to-eigen.h
stack-template.h
stack-template.t.cpp
......@@ -89,6 +95,7 @@ SET(headers
task-dyn-joint-limits.h
task-dyn-limits.h
task-dyn-inequality.h
task-dyn-passing-point.h
solver-op-space.h
solver-dyn-reduced.h
......@@ -107,6 +114,7 @@ SET(headers
list(APPEND task-dyn-limits_plugins_dependencies task-dyn-pd)
list(APPEND task-dyn-inequality_plugins_dependencies task-dyn-pd)
list(APPEND task-dyn-joint-limits_plugins_dependencies task-dyn-pd)
list(APPEND task-dyn-passing-point_plugins_dependencies task-dyn-pd)
list(APPEND pseudo-robot-dynamic_plugins_dependencies dynamic-integrator)
list(APPEND contact-selecter_plugins_dependencies solver-dyn-reduced)
......@@ -132,21 +140,16 @@ foreach(lib solver-op-space solver-dyn-reduced solver-dyn-red2)
ENDIF(WIN32)
endforeach(lib)
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
FIND_NUMPY()
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
ENDIF(BUILD_PYTHON_INTERFACE)
# Add subdirectories.
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(unitTesting)
# Configure some of the python files
CONFIGURE_FILE(
"${CMAKE_CURRENT_SOURCE_DIR}/python/robot_specific.py.cmake"
"${CMAKE_CURRENT_BINARY_DIR}/python/robot_specific.py"
)
#INCLUDE(cmake/python.cmake)
#INSTALL(FILES
# "${CMAKE_CURRENT_BINARY_DIR}/python/robotSpecific.py"
# DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dyninv
#)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
README.md
View file @ 60cf95b2
sot-dyninv
==========
[![Building Status](https://travis-ci.org/stack-of-tasks/sot-dyninv.svg?branch=master)](https://travis-ci.org/stack-of-tasks/sot-dyninv)
[![Pipeline status](https://gepgitlab.laas.fr/stack-of-tasks/sot-dyninv/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/stack-of-tasks/sot-dyninv/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/stack-of-tasks/sot-dyninv/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/stack-of-tasks/sot-dyninv/master/coverage/)
This sofware provides the solver and features to resolve the inverse of the
dynamics of a free-floating robot, in contact with its environment. The basics
are provided by the sot core, and the computation of the dynamics of the robot
......@@ -36,5 +41,5 @@ have to be available on your machine.
- usual compilation tools (GCC/G++, make, etc.)
[sot-core]: http://github.com/jrl-umi3128/sot-core
[soth]: ssh://softs.laas.fr/git/jrl/frameworks/soth
[sot-core]: http://github.com/stack-of-tasks/sot-core
[soth]: https://github.com/stack-of-tasks/soth.git
cmake @ b58bd669
Compare 33e7189d...b58bd669
Subproject commit 33e7189de7eb7bcd9fd5d8409aac413d0c8fa6f4
Subproject commit b58bd669f6567662eefb8a410e8e40aeba4f1060
doc/CMakeLists.txt deleted 100644 → 0
View file @ 19962da3
#
# Copyright 2011 CNRS
# Author: Nicolas Mansard (empty doc file ... who else?)
#
doc/Doxyfile.extra.in
View file @ 60cf95b2
INPUT = @CMAKE_SOURCE_DIR@/include \
@CMAKE_SOURCE_DIR@/src \
@CMAKE_SOURCE_DIR@/doc/additionalDoc
IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc/pictures
FILE_PATTERNS = *.cc *.cpp *.h *.hh *.hxx
TAGFILES = \
"@JRL_MAL_DOXYGENDOCDIR@/jrl-mal.doxytag = @JRL_MAL_DOXYGENDOCDIR@" \
"@DYNAMIC_GRAPH_DOXYGENDOCDIR@/dynamic-graph.doxytag = @DYNAMIC_GRAPH_DOXYGENDOCDIR@" \
"@SOT_CORE_DOXYGENDOCDIR@/sot-core.doxytag = @SOT_CORE_DOXYGENDOCDIR@"
doc/additionalDoc/package.hh
View file @ 60cf95b2
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
* This file is part of sot-dynamic.
* sot-dynamic 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-dynamic 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 Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
*/
/** \mainpage
\section sot_dynamic_section_introduction Introduction
The sot-dynamic package is a bridge between the stack of tasks framework and the dynamicsJRLJapan library.
It provides an inverse dynamic model of the robot through dynamic-graph entities.
More precisely it wraps the newton euler algorithm implemented by the dynamicsJRLJapan library
to make it accessible for the stack of tasks controllers
(in the Stack of Tasks Framework as defined in \ref Mansard2007.)
This package depends on the following packages:
\li dynamicsJRLJapan
\li sot-core
\li dynamic-graph
\li dynamic-graph-python
Optional packages (for specific support of the hrp2-N robots)
\li hrp210optimized
\li hrp2-dynamics
See the JRL umi3218's page on github for instructions on how to download and
install these packages at https://github.com/jrl-umi3218.
\section python_bindings Python bindings
As most packages based on the dynamic-graph framework (see https://github.com/jrl-umi3218/dynamic-graph),
the functionnality is exposed through entities. Hence python sub-modules of dynamic_graph are generated. See <a href="../sphinx-html/index.html">sphinx documentation</a> for more details.
The following entities are created by this package:\n
(all entites are placed in the namespace sot::)
\li sot::ZmprefFromCom
\li sot::ForceCompensation
\li sot::IntegratorForceExact
\li sot::MassApparent
\li sot::IntegratorForceRk4
\li sot::IntegratorForce
\li sot::AngleEstimator
\li sot::WaistAttitudeFromSensor
\li sot::Dynamic - provides the inverse dynamics computations for of a humanoid robot
Optionally, if the packages in brackets are installed, the following entities
are made available:
\li sot::DynamicHrp2 - same as sot::Dynamic, but specialized for hrp2 robots [needs hrp2-dynamics]
\li sot::DynamicHrp2_10 - same as previous, optimized for the hrp2-10 robot [needs hrp210optimized]
\li sot::DynamicHrp2_10_old [needs hrp210optimized]
See each entity's documentation page for more information (when available).
\section References
\anchor Mansard2007
<b>"Task sequencing for sensor-based control"</b>,
<em>N. Mansard, F. Chaumette,</em>
IEEE Trans. on Robotics, 23(1):60-72, February 2007
**/
include/CMakeLists.txt
View file @ 60cf95b2
......@@ -18,7 +18,6 @@ SET(${PROJECT_NAME}_HEADERS
entity-helper.h
signal-helper.h
mal-to-eigen.h
stack-template.h
stack-template.t.cpp
......
python/CMakeLists.txt 0 → 100644
View file @ 60cf95b2
IF(BUILD_PYTHON_INTERFACE)
CONFIGURE_FILE(
"${CMAKE_CURRENT_SOURCE_DIR}/robot_specific.py.cmake"
"${CMAKE_CURRENT_BINARY_DIR}/robot_specific.py"
)
INSTALL(FILES
${CMAKE_CURRENT_BINARY_DIR}/robot_specific.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dyninv
)
INSTALL(
FILES ros/sot-concept.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/tutorial
)
ENDIF(BUILD_PYTHON_INTERFACE)
python/ros/ros-kineromeo.py deleted 100644 → 0
View file @ 19962da3
# -*- 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/>.
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
# Create the robot romeo.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('romeo', device=RobotSimu('romeo'))
plug(robot.device.state, robot.dynamic.position)
# Binds with ROS. assert that roscore is running.
from dynamic_graph.ros import *
ros = Ros(robot)
# Create a simple kinematic solver.
from dynamic_graph.sot.dynamics.solver import Solver
solver = Solver(robot)
# Alternate visualization tool
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer
addRobotViewer(robot.device,small=True,small_extra=24,verbose=False)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
# define the macro allowing to run the simulation.
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
dt=5e-3
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
# Defines a task for the right hand.
taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
# Move the operational point.
handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# robot.tasks['right-wrist'].add(taskRH.feature.name)
# --- STATIC COM (if not walking)
robot.createCenterOfMassFeatureAndTask(
'featureCom', 'featureComDef', 'comTask',
selec = '011',
gain = 10)
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact'+name] = contact
# --- RUN ----------------------------------------------------------------------
# Move the desired position of the right hand
# 1st param: task concerned
# 2st param: objective
# 3rd param: selected parameters
# 4th param: gain
target=(0.5,-0.2,0.8)
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
# Fill the stack with some tasks.
solver.push(contactRF.task)
solver.push(contactLF.task)
solver.push(robot.comTask)
solver.push(taskRH.task)
# type inc to run one iteration, or go to run indefinitely.
# go()
python/ros/ros-walkromeo.py deleted 100644 → 0
View file @ 19962da3
# ______________________________________________________________________________
# ******************************************************************************
# A simple Herdt walking pattern generator for Romeo.
# ______________________________________________________________________________
# ******************************************************************************
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.dyninv import SolverKine
from numpy import *
def totuple( a ):
al=a.tolist()
res=[]
for i in range(a.shape[0]):
res.append( tuple(al[i]) )
return tuple(res)
# Create the robot.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('romeo', device=RobotSimu('romeo'))
plug(robot.device.state, robot.dynamic.position)
# Binds with ros, export joint_state.
from dynamic_graph.ros import *
ros = Ros(robot)
# Create a solver.
from dynamic_graph.sot.dynamics.solver import Solver
solver = Solver(robot)
# Alternate visualization tool
addRobotViewer(robot.device,small=True,small_extra=24,verbose=False)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
dt=5e-3
@loopInThread
def inc():
robot.device.increment(dt)
updateComDisplay(robot.device,robot.dynamic.com)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
pg = MetaPG(robot.dynamic)
pg.plugZmp(robot.device)
# ---- TASKS -------------------------------------------------------------------
# ---- WAIST TASK ---
# This task fix the motion of the waist around the z axis, the roll and the pitch
taskWaist=MetaTask6d('waist',robot.dynamic,'waist','waist')
pg.plugWaistTask(taskWaist)
taskWaist.task.controlGain.value = 5
taskWaist.feature.selec.value = '011100'
# --- TASK COM ---
# the x,y coords of the centor of mass are given by the pattern generator
taskCom = MetaTaskKineCom(robot.dynamic,"compd")
pg.plugComTask(taskCom)
taskCom.feature.selec.value = '011'
# --- TASK FEET
# The feet are constrained by two 6dofs tasks.
taskRF=MetaTask6d('rf',robot.dynamic,'rf','right-ankle')
plug(pg.pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 40
taskLF=MetaTask6d('lf',robot.dynamic,'lf','left-ankle')
plug(pg.pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 40
############################################################################
# Complete the task definition here.
# ---- HEAD ORIENTATION ---
# set the orientation of the gaze (head) to be the same as the one of the foot.
# Define a metaTask for a 6d task controlling the waistOrientation.
# 1\ Define a MetaTask6d taskHead, constraining the head, attached to the gaze link
taskHead=MetaTask6d('head',robot.dynamic,'gaze','gaze')
# 2\ Link the orientation of the right foot to the desired position of the head.
# 3\ Only constraint the rotation of the head.
# 4\ set the gain
# ---- ARMS ---
# set a default position for the joints arms
# 1\ Define two features (entity FeatureGeneric) corresponding to the desired potion.
# the desired position corresponds to the initial configuration of the robot
# romeo: initialConfig['romeo']
# * the reference position is the position given by the entity dyn,
# * the jacobian is the identity: ... = totuple( identity(robot.dimension) )
# * the joint controlled are those of the two arms [12,25] sur 39.
# 2\ Define the task. Associate to the task the position feature.
# 3\ (Optional) attach an adaptive gain (entity GainAdaptive) to the task created.
############################################################################
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
solver.push(taskWaist.task)
solver.push(taskRF.task)
solver.push(taskLF.task)
solver.push(taskCom.task)
# Activate your new tasks.
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt(False)
print('You can now modifiy the speed of the robot by setting pg.pg.velocitydes')
print('example : pg.pg.velocitydes.value =(0.1,0.0,0.0)\n')
go()
python/ros/sot-concept.py
View file @ 60cf95b2
......@@ -26,7 +26,7 @@ from numpy import *
# create the robot and plug main signal.
from dynamic_graph.sot.romeo.romeo import *
from dynamic_graph.sot.romeo.robot import *
robot = Robot('romeo', device=RobotSimu('romeo'))
plug(robot.device.state, robot.dynamic.position)
......@@ -325,8 +325,15 @@ ball.move((0.5,-0.2,1.0),0)
next()
next()
next()
import time
def config(ref=0):
# reset the robot configuration
stop() # stop the control loop
time.sleep(0.1) # wait (pb of sync with the viewer)
robot.device.set(robot.halfSitting)
next() # manual reset of the robot's posture
if ref==0:
print '''Only the task RH'''
elif ref==1:
......@@ -411,6 +418,8 @@ def menu(ref=0):
print '''6: Task RH + foot constraint + COM= + SINGULARITE '''
print '''7: Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
print ''
print '''Please re-type menu() to display this choice again'''
print ''
uinp = raw_input(' Config? >> ')
config(int(uinp))
......
python/unittests/dynSimpleTest.py 0 → 100644
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
#
# BASIC EXAMPLE OF THE DYNAMIC SOT
# Robot: HRP-2 N.14
# Tasks: Rotate the head and move the arms
#
# ______________________________________________________________________________
# ******************************************************************************
from dynamic_graph.sot.core import *
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph import plug
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger
from dynamic_graph.sot.core.utils.thread_interruptible_loop import *
from dynamic_graph.sot.core.utils.attime import attime
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir, modelName, robotDimension, initialConfig, gearRatio, inertiaRotor
from dynamic_graph.sot.dyninv.meta_task_dyn_6d import MetaTaskDyn6d
from dynamic_graph.sot.dyninv.meta_tasks_dyn import MetaTaskDynCom, MetaTaskDynPosture, MetaTaskDynLimits, AddContactHelper, gotoNd
from numpy import *
# ------------------------------------------------------------------------------
# --- ROBOT DYNAMIC SIMULATION -----------------------------------------------
# ------------------------------------------------------------------------------
robotName = 'hrp14small'
robotDim = robotDimension[robotName]
RobotClass = RobotDynSimu
robot = RobotClass("robot")
robot.resize(robotDim)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,verbose=False)
dt=5e-3
# ------------------------------------------------------------------------------
# --- MAIN LOOP --------------------------------------------------------------
# ------------------------------------------------------------------------------
def inc():
robot.increment(dt)
attime.run(robot.control.time)
@loopInThread
def loop():
inc()
runner=loop()
# --- shortcuts -------------------------------------------------
@optionalparentheses
def go(): runner.play()
@optionalparentheses
def stop(): runner.pause()
@optionalparentheses
def next(): inc()
@optionalparentheses
def iter(): print 'iter = ',robot.state.time
@optionalparentheses
def status(): print runner.isPlay
# ----------------------------------------------------------------------------
# ---- DYN -----------------------------------------------------------------
# ----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
dyn = Dynamic("dyn")
dyn.setFiles(modelDir,modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
dyn.acceleration.value = robotDim*(0.,)
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
# ----------------------------------------------------------------------------
# --- TASKS (for HRP2-14) --------------------------------------------------
# ----------------------------------------------------------------------------
# Operational Point (6D) Tasks
taskWaist = MetaTaskDyn6d('taskWaist', dyn, 'waist', 'waist')
taskChest = MetaTaskDyn6d('taskChest', dyn, 'chest', 'chest')
taskHead = MetaTaskDyn6d('taskHead', dyn, 'head', 'gaze')
taskrh = MetaTaskDyn6d('rh', dyn, 'rh', 'right-wrist')
tasklh = MetaTaskDyn6d('lh', dyn, 'lh', 'left-wrist')
for task in [ taskWaist, taskChest, taskHead, taskrh, tasklh]:
task.feature.frame('current')
task.gain.setConstant(50)
task.task.dt.value = dt
task.featureDes.velocity.value=(0,0,0,0,0,0)
# CoM Task
taskCom = MetaTaskDynCom(dyn,dt)
# Posture Task
taskPosture = MetaTaskDynPosture(dyn,dt)
# Angular position and velocity limits
taskLim = MetaTaskDynLimits(dyn,dt)
# ----------------------------------------------------------------------------
# --- Dynamic Stack of Tasks (SoT) controller ------------------------------
# ----------------------------------------------------------------------------
sot = SolverDynReduced('sot')
contact = AddContactHelper(sot)
sot.setSize(robotDim-6)
sot.breakFactor.value = 10
plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
plug(dyn.velocity,sot.velocity)
plug(sot.solution, robot.control)
plug(sot.acceleration,robot.acceleration)
# ----------------------------------------------------------------------------
# ---- CONTACT: Contact definition -----------------------------------------
# ----------------------------------------------------------------------------
# Left foot contact
contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle')
contactLF.featureDes.velocity.value=(0,0,0,0,0,0)
contactLF.feature.frame('desired')
contactLF.name = "LF"
# Right foot contact
contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle')
contactRF.featureDes.velocity.value=(0,0,0,0,0,0)
contactRF.feature.frame('desired')
contactRF.name = "RF"
contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-0.105))
contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-0.105,-0.105,-0.105))
# Imposed erordot = 0
contactLF.feature.errordot.value = (0,0,0,0,0,0)
contactRF.feature.errordot.value = (0,0,0,0,0,0)
# -----------------------------------------------------------------------------
# --- TRACES AND AUTO RECOMPUTING SIGNALS -----------------------------------
# -----------------------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','dyn_','.dat')
tr.add('sot.acceleration','')
tr.add('sot.torque','')
tr.add('sot.reducedControl','')
tr.add('sot.reducedForce','')
tr.add('sot.forces','')
tr.add('sot.forcesNormal','')
tr.add('sot.reducedForce','')
tr.start()
robot.after.addSignal('sot.reducedForce')
robot.after.addSignal('sot.forces')
robot.after.addSignal('sot.torque')
robot.after.addSignal('tr.triger')
robot.after.addSignal(contactLF.task.name+'.error')
robot.after.addSignal('dyn.rf')
robot.after.addSignal('dyn.lf')
robot.after.addSignal('dyn.chest')
robot.after.addSignal('dyn.lh')
robot.after.addSignal('dyn.com')
robot.after.addSignal('sot.forcesNormal')
robot.after.addSignal('dyn.waist')
robot.after.addSignal(taskLim.task.name+'.normalizedPosition')
# ----------------------------------------------------------------------------
# --- RUN ------------------------------------------------------------------
# ----------------------------------------------------------------------------
dyn.lh.recompute(0)
dyn.rh.recompute(0)
lh0 = tuple([x[3] for x in dyn.lh.value])
rh0 = tuple([x[3] for x in dyn.rh.value])
r = radians
sot.clear()
contact(contactLF)
contact(contactRF)
sot.push(taskLim.task.name)
plug(robot.state,sot.position)
attime(2
,(lambda: sot.push(taskChest.task.name), "Add Chest to the SoT")
,(lambda: taskChest.feature.keep(), "Keep Chest")
,(lambda: sot.push(taskHead.task.name), "Add Head to the SoT")
,(lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), -r(40)), "111000"), "Rotate Head to the right")
,(lambda: sot.push(taskrh.task.name), "Add Right hand to the SoT")
,(lambda: gotoNd(taskrh, (rh0[0]-0.3, rh0[1]-0.4, 0), "000011"), "Move right hand")
)
attime(200
,(lambda: gotoNd( taskHead, (0, 0, 0, 0, r(25), r(40)), "111000"), "Rotate Head to the left")
,(lambda: sot.push(tasklh.task.name), "Add Left hand to the SoT")
,(lambda: gotoNd(tasklh, (lh0[0]+0.3, lh0[1]+0.4, 0), "000011"), "Move left hand")
)
attime(400
,(lambda: gotoNd(taskHead, (0, 0, 0, 0, 0, 0), "111000"), "Rotate Head to the center")
,(lambda: gotoNd(tasklh, (lh0[0], lh0[1], 0), "000011"), "Return left hand to initial position")
,(lambda: gotoNd(taskrh, (rh0[0], rh0[1], 0), "000011"), "Return right hand to initial position")
)
attime(600, stop, "Stopped")
go()
python/unittests/kinesimple.py
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
#
# The simplest robot task: just go and reach a point with the right hand.
#
# ______________________________________________________________________________
# ******************************************************************************
......@@ -17,29 +19,35 @@ from numpy import *
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
# --- ROBOT SIMU ---------------------------------------------------------------
# ------------------------------------------------------------------------------
# --- ROBOT SIMULATION ---------------------------------------------------------
# ------------------------------------------------------------------------------
robotName = 'hrp14small'
robotDim=robotDimension[robotName]
robot = RobotSimu("robot")
robotDim = robotDimension[robotName]
robot = RobotSimu("robot")
robot.resize(robotDim)
dt=5e-3
from dynamic_graph.sot.dyninv.robot_specific import halfSittingConfig
x0=-0.00949035111398315034
y0=0
z0=0.64870185118253043
x0 = -0.00949035111398315034
y0 = 0
z0 = 0.64870185118253043
halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
q0=list(halfSittingConfig[robotName])
initialConfig[robotName]=tuple(q0)
q0 = list(halfSittingConfig[robotName])
initialConfig[robotName] = tuple(q0)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,verbose=True)
addRobotViewer(robot, small=True, verbose=True)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
......@@ -48,11 +56,13 @@ def inc():
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
......@@ -67,21 +77,23 @@ plug(robot.state,dyn.position)
dyn.velocity.value = robotDim*(0.,)
dyn.acceleration.value = robotDim*(0.,)
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
sot = SOT('sot')
sot.setNumberDofs(robotDim)
sot.setSize(robotDim)
plug(sot.control,robot.control)
# ------------------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ------------------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
# ---- TASK GRIP
taskRH = MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip = eye(4); handMgrip[0:3,3] = (0,0,-0.21)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
......@@ -100,9 +112,12 @@ for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact.keep()
locals()['contact'+name] = contact
# --- RUN ----------------------------------------------------------------------
target=(0.5,-0.2,1.3)
# ----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
# ----------------------------------------------------------------------------
target = (0.5,-0.2,1.3)
robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
......
python/unittests/walkromeo.py deleted 100644 → 0
View file @ 19962da3
# ______________________________________________________________________________
# ******************************************************************************
# A simple Herdt walking pattern generator for Romeo.
# ______________________________________________________________________________
# ******************************************************************************
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
from dynamic_graph.sot.dyninv import SolverKine
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor,specificitiesName,jointRankName
from numpy import *
def totuple( a ):
al=a.tolist()
res=[]
for i in range(a.shape[0]):
res.append( tuple(al[i]) )
return tuple(res)
# --- ROBOT SIMU ---------------------------------------------------------------
robotName = 'romeo'
robotDim=robotDimension[robotName]
robot = RobotSimu("romeo")
robot.resize(robotDim)
dt=5e-3
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,small_extra=24,verbose=False)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
@loopInThread
def inc():
robot.increment(dt)
updateComDisplay(robot,dyn.com)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
#---- DYN --------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/' + specificitiesName[robotName]
jointRankPath = xmlDir + '/' + jointRankName[robotName]
dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state,dyn.position)
dyn.velocity.value = robotDim*(0.,)
dyn.acceleration.value = robotDim*(0.,)
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
pg = MetaPG(dyn)
pg.plugZmp(robot)
# ---- SOT ---------------------------------------------------------------------
# The basic SOT solver would work too.
sot = SolverKine('sot')
sot.setSize(robotDim)
plug(sot.control,robot.control)
# ---- TASKS -------------------------------------------------------------------
# ---- WAIST TASK ---
taskWaist=MetaTask6d('waist',dyn,'waist','waist')
pg.plugWaistTask(taskWaist)
taskWaist.task.controlGain.value = 5
taskWaist.feature.selec.value = '011100'
# --- TASK COM ---
taskCom = MetaTaskKineCom(dyn,"compd")
pg.plugComTask(taskCom)
taskCom.feature.selec.value = '011'
# --- TASK FEET
taskRF=MetaTask6d('rf',dyn,'rf','right-ankle')
plug(pg.pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 40
taskLF=MetaTask6d('lf',dyn,'lf','left-ankle')
plug(pg.pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 40
# ---- WAIST TASK ORIENTATION ---
# set the orientation of the waist to be the same as the one of the foot.
taskWaistOr=MetaTask6d('waistOr',dyn,'waist','waist')
plug(pg.pg.rightfootref,taskWaistOr.featureDes.position)
taskWaistOr.task.controlGain.value = 40
taskWaistOr.feature.selec.value = '100000'
taskHead=MetaTask6d('head',dyn,'gaze','gaze')
plug(taskRF.featureDes.position, taskHead.featureDes.position)
taskHead.feature.selec.value = '111000'
taskHead.task.controlGain.value = 5
# --- TASK POSTURE --------------------------------------------------
# set a default position for the joints.
featurePosition = FeatureGeneric('featurePosition')
featurePositionDes = FeatureGeneric('featurePositionDes')
featurePosition.setReference('featurePositionDes')
plug(dyn.position,featurePosition.errorIN)
featurePositionDes.errorIN.value = initialConfig['romeo']
featurePosition.jacobianIN.value = totuple( identity(robotDim) )
taskPosition = Task('taskPosition')
taskPosition.add('featurePosition')
# featurePosition.selec.value = toFlags((6,24))
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1,0.1,125e3)
gainPosition.gain.value = 5
plug(taskPosition.error,gainPosition.error)
plug(gainPosition.gain,taskPosition.controlGain)
featurePosition.selec.value = '000000000000001111111111111100000000000'
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskWaistOr.task.name)
# Stun the upper part of the body.
sot.push(taskHead.task.name) # constraint the head orientation: look straight ahead
sot.push('taskPosition') # stun the arms.
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt(False)
# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
pg.pg.velocitydes.value =(0.1,0.0,0.0)
go()
src/CMakeLists.txt
View file @ 60cf95b2
......@@ -14,9 +14,6 @@
# received a copy of the GNU Lesser General Public License along with
# sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
INCLUDE(../cmake/python.cmake)
FINDPYTHON()
IF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
ADD_DEFINITIONS(-DDEBUG=2)
ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
......@@ -33,11 +30,12 @@ FOREACH(header ${headers})
ELSE(WIN32)
execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink ${src_path} ${bin_path} )
ENDIF(WIN32)
install(FILES ${src_path} DESTINATION ${install_path}
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
install(FILES ${src_path} DESTINATION ${install_path}
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
ENDFOREACH(header)
INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/include)
LINK_DIRECTORIES(${PYTHON_LIBRARY_DIRS})
# --- COMPILATION OF PLUGINS
FOREACH(lib ${libs})
......@@ -60,22 +58,27 @@ FOREACH(lib ${libs})
PKG_CONFIG_USE_DEPENDENCY(${lib} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${lib} soth)
INSTALL(TARGETS ${lib} DESTINATION lib/plugin)
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS ${lib} DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${lib})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/dyninv/${PYTHON_LIBRARY_NAME}"
${lib}
sot-dyninv-${PYTHON_LIBRARY_NAME}-wrap
)
IF(BUILD_PYTHON_INTERFACE)
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${lib})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/dyninv/${PYTHON_LIBRARY_NAME}"
${lib}
sot-dyninv-${PYTHON_LIBRARY_NAME}-wrap
)
ENDIF(BUILD_PYTHON_INTERFACE)
ENDFOREACH(lib)
# Install empty __init__.py files in intermediate directories.
INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_task_dyn_6d.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn.py
${CMAKE_CURRENT_BINARY_DIR}/../python/robot_specific.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dyninv
)
MESSAGE(STATUS DESTINATION ${PYTHON_SITELIB} )
IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_task_dyn_6d.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_task_dyn_passing_point.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dyninv
)
ENDIF(BUILD_PYTHON_INTERFACE)