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 (327)
227 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 1532 additions and 88 deletions
.gitignore 0 → 100644
View file @ 60cf95b2
*build*
*~
*.pyc
*/#*#
.#*
*/octave-core
matlab
*dat
release
python/portability.cmake
.gitlab-ci.yml 0 → 100644
View file @ 60cf95b2
include: http://rainboard.laas.fr/project/sot-dyninv/.gitlab-ci.yml
.gitmodules 0 → 100644
View file @ 60cf95b2
[submodule "cmake"]
path = cmake
url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
.mailmap 0 → 100644
View file @ 60cf95b2
#
# This list is used by git-shortlog to fix a few botched name translations
# in the git archive, either because the author's full name was messed up
# and/or not always written the same way, making contributions from the
# same person appearing not to be so or badly displayed.
Anthony Mallet <anthony.mallet@laas.fr>
Olivier Stasse <grxuser@jrl005.a01.aist.go.jp>
Olivier Stasse <olivier.stasse@aist.go.jp>
Olivier Stasse <stasse@dhcpt1243.a02.aist.go.jp>
Olivier Stasse <stasse@jrl005.a01.aist.go.jp>
Olivier Stasse <olivier.stasse@aist.go.jp>
Olivier Stasse <stasse@stasse-laptop.(none)>
Olivier Stasse <olivier.stasse@aist.go.jp>
Olivier Stasse <stasse@dhcpt1059.a02.aist.go.jp>
Olivier Stasse <stasse@jrl005.a01.aist.go.jp>
Olivier Stasse <stasse@jrl005.a02.aist.go.jp>
Olivier Stasse <olivier.stasse@aist.go.jp>
Olivier Stasse <stasse@jrl006.(none)>
Olivier Stasse <stasse@stasse-laptop.(none)>
Nicolas Mansard <nmansard@devgiri.(none)>
Nicolas Mansard <nmansard@jorasse.(none)>
Nicolas Mansard <nmansard@laas.fr>
Nicolas Mansard <nicolas.mansard@laas.fr>
Florent Lamiraux <florent@big-laptop.(none)>
Florent Lamiraux <florent@dhcpt1253.a02.aist.go.jp>
Florent Lamiraux <florent@florent-laptop.laas.fr>
Florent Lamiraux <florent@florent-laptop.(none)>
Florent Lamiraux <florent@laas.fr>
Florent Lamiraux <Florent Lamiraux florent@laas.fr>
Florent Lamiraux <florent@florent-laptop.(none)>
Florent Lamiraux <Florent Lamiraux florent@laas.fr>
Florent Lamiraux <florent@localhost.localdomain>
Florent Lamiraux <florent@poullenc.laas.fr>
Florent Lamiraux <florent@poullenc.(none)>
.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,24 +39,117 @@ 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
controller-pd
dynamic-integrator
zmp-estimator
pseudo-robot-dynamic
robot-dyn-simu
controller-pd
task-dyn-pd
task-dyn-joint-limits
task-dyn-limits
task-dyn-inequality
task-dyn-passing-point
solver-op-space
solver-dyn-reduced
solver-kine
task-joint-limits
task-inequality
task-weight
feature-projected-line
contact-selecter
)
LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
SET(headers
commands-helper.h
entity-helper.h
signal-helper.h
dynamic-integrator.h
zmp-estimator.h
pseudo-robot-dynamic.h
robot-dyn-simu.h
stack-template.h
stack-template.t.cpp
controller-pd.h
task-dyn-pd.h
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
task-joint-limits.h
task-inequality.h
task-weight.h
solver-kine.h
feature-projected-line.h
col-piv-qr-solve-in-place.h
contact-selecter.h
)
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)
foreach(lib task-weight task-inequality task-dyn-pd task-joint-limits)
IF(WIN32)
list(APPEND ${lib}_ext_plugins_dependencies
${DYNAMIC_GRAPH_PLUGINDIR}/task${CMAKE_LINK_LIBRARY_SUFFIX})
ELSE(WIN32)
list(APPEND ${lib}_ext_plugins_dependencies
${DYNAMIC_GRAPH_PLUGINDIR}/task${CMAKE_SHARED_LIBRARY_SUFFIX})
ENDIF(WIN32)
endforeach(lib)
foreach(lib solver-op-space solver-dyn-reduced solver-dyn-red2)
list(APPEND ${lib}_plugins_dependencies task-dyn-pd)
IF(WIN32)
list(APPEND ${lib}_ext_plugins_dependencies
${DYNAMIC_GRAPH_PLUGINDIR}/feature-point6d${CMAKE_LINK_LIBRARY_SUFFIX})
ELSE(WIN32)
list(APPEND ${lib}_ext_plugins_dependencies
${DYNAMIC_GRAPH_PLUGINDIR}/feature-point6d${CMAKE_SHARED_LIBRARY_SUFFIX})
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(include)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(unitTesting)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
\ No newline at end of file
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
......@@ -29,10 +34,12 @@ have to be available on your machine.
- Libraries:
- sot-core (>= 1.0.0)
- soth (>= 0.0.1)
- System tools:
- CMake (>=2.6)
- pkg-config
- usual compilation tools (GCC/G++, make, etc.)
[sot-core]: http://github.com/jrl-umi3128/sot-core
[sot-core]: http://github.com/stack-of-tasks/sot-core
[soth]: https://github.com/stack-of-tasks/soth.git
cmake @ b58bd669
Subproject commit b58bd669f6567662eefb8a410e8e40aeba4f1060
doc/CMakeLists.txt deleted 100644 → 0
View file @ c23a9a54
#
# 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
......@@ -14,7 +14,19 @@
# sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
SET(${PROJECT_NAME}_HEADERS
commands-helper.h
entity-helper.h
signal-helper.h
stack-template.h
stack-template.t.cpp
controller-pd.h
task-dyn-pd.h
dynamic-integrator.h
solver-op-space.h
zmp-estimator.h
pseudo-robot-dynamic.h
)
# Recreate correct path for the headers
......
kinewalk.py 0 → 100644
View file @ 60cf95b2
import sys
sys.path.append('../../sot-dyninv/python')
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.dynamics import *
import dynamic_graph.script_shortcuts
from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph.matlab import matlab
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
robotName = 'hrp10small'
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)
initialConfig.clear()
initialConfig['hrp10small'] = (0,0,0.648697398115,0,0,0)+(0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, -0.4538, 0.8727, -0.4189, 0, 0, 0, 0, 0, 0.2618, -0.1745, 0, -0.5236, 0, 0, 0, 0, 0.2618, 0.1745, 0, -0.5236, 0, 0, 0, 0 )
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
robotDim=robotDimension[robotName]
robot = RobotSimu("robot")
robot.resize(robotDim)
robot.set( initialConfig[robotName] )
dt=5e-3
# --- VIEWER -------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
try:
import robotviewer
def stateFullSize(robot):
return [float(val) for val in robot.state.value]+10*[0.0]
RobotSimu.stateFullSize = stateFullSize
robot.viewer = robotviewer.client('XML-RPC')
# Check the connection
robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
def refreshView( robot ):
robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
RobotSimu.refresh = refreshView
def incrementView( robot,dt ):
robot.incrementNoView(dt)
robot.refresh()
RobotSimu.incrementNoView = RobotSimu.increment
RobotSimu.increment = incrementView
def setView( robot,*args ):
robot.setNoView(*args)
robot.refresh()
RobotSimu.setNoView = RobotSimu.set
RobotSimu.set = setView
robot.refresh()
except:
print "No robot viewer, sorry."
robot.viewer = None
# --- MAIN LOOP ------------------------------------------
qs=[]
def inc():
robot.increment(dt)
qs.append(robot.state.value)
from ThreadInterruptibleLoop import *
@loopInThread
def loop():
inc()
runner=loop()
@optionalparentheses
def go(): runner.play()
@optionalparentheses
def stop(): runner.pause()
@optionalparentheses
def next(): inc() #runner.once()
# --- shortcuts -------------------------------------------------
@optionalparentheses
def n():
inc()
qdot()
@optionalparentheses
def n5():
for loopIdx in range(5): inc()
@optionalparentheses
def n10():
for loopIdx in range(10): inc()
@optionalparentheses
def q():
if 'dyn' in globals(): print dyn.ffposition.__repr__()
print robot.state.__repr__()
@optionalparentheses
def qdot(): print robot.control.__repr__()
@optionalparentheses
def t(): print robot.state.time-1
@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)
dyn.velocity.value = robotDim*(0.,)
dyn.acceleration.value = robotDim*(0.,)
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
robot.control.unplug()
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator import PatternGenerator,Selector
pg = PatternGenerator('pg')
pg.setVrmlDir(modelDir+'/')
pg.setVrml(modelName[robotName])
pg.setXmlSpec(specificitiesPath)
pg.setXmlRank(jointRankPath)
pg.buildModel()
# Standard initialization
pg.parseCmd(":samplingperiod 0.005")
pg.parseCmd(":previewcontroltime 1.6")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":omega 0.0")
pg.parseCmd(":stepheight 0.05")
pg.parseCmd(":singlesupporttime 0.780")
pg.parseCmd(":doublesupporttime 0.020")
pg.parseCmd(":armparameters 0.5")
pg.parseCmd(":LimitsFeasibility 0.0")
pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0")
pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa")
plug(dyn.position,pg.position)
plug(dyn.com,pg.com)
pg.motorcontrol.value = robotDim*(0,)
pg.zmppreviouscontroller.value = (0,0,0)
pg.initState()
# --- PG INIT FRAMES ---
geom = Dynamic("geom")
geom.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
geom.parse()
geom.createOpPoint('rf','right-ankle')
geom.createOpPoint('lf','left-ankle')
plug(dyn.position,geom.position)
geom.ffposition.value = 6*(0,)
geom.velocity.value = robotDim*(0,)
geom.acceleration.value = robotDim*(0,)
# --- Selector of Com Ref: when pg is stopped, pg.inprocess becomes 0
comRef = Selector('comRef'
,['vector','ref',dyn.com,pg.comref])
plug(pg.inprocess,comRef.selec)
selecSupportFoot = Selector('selecSupportFoot'
,['matrixHomo','pg_H_sf',pg.rightfootref,pg.leftfootref]
,['matrixHomo','wa_H_sf',geom.rf,geom.lf]
)
plug(pg.SupportFoot,selecSupportFoot.selec)
sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
plug(selecSupportFoot.wa_H_sf,sf_H_wa.sin)
pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
plug(selecSupportFoot.pg_H_sf,pg_H_wa.sin1)
plug(sf_H_wa.sout,pg_H_wa.sin2)
# --- Compute the ZMP ref in the Waist reference frame.
wa_H_pg = Inverse_of_matrixHomo('wa_H_pg')
plug(pg_H_wa.sout,wa_H_pg.sin)
wa_zmp = Multiply_matrixHomo_vector('wa_zmp')
plug(wa_H_pg.sout,wa_zmp.sin1)
plug(pg.zmpref,wa_zmp.sin2)
# Connect the ZMPref to OpenHRP in the waist reference frame.
pg.parseCmd(':SetZMPFrame world')
plug(wa_zmp.sout,robot.zmp)
# ---- TASKS -------------------------------------------------------------------
# ---- WAIST TASK ---
taskWaist=MetaTask6d('waist',dyn,'waist','waist')
# Build the reference waist pos homo-matrix from PG.
waistReferenceVector = Stack_of_vector('waistReferenceVector')
plug(pg.initwaistposref,waistReferenceVector.sin1)
plug(pg.initwaistattref,waistReferenceVector.sin2)
waistReferenceVector.selec1(0,3)
waistReferenceVector.selec2(0,3)
waistReference=PoseRollPitchYawToMatrixHomo('waistReference')
plug(waistReferenceVector.sout,waistReference.sin)
plug(waistReference.sout,taskWaist.featureDes.position)
taskWaist.feature.selec.value = '011100'
taskWaist.task.controlGain.value = 5
# --- TASK COM ---
featureCom = FeatureGeneric('featureCom')
plug(dyn.com,featureCom.errorIN)
plug(dyn.Jcom,featureCom.jacobianIN)
featureComDes = FeatureGeneric('featureComDes')
featureCom.setReference('featureComDes')
plug(comRef.ref,featureComDes.errorIN)
featureCom.selec.value = '011'
taskComPD = TaskPD('taskComPD')
taskComPD.add('featureCom')
plug(pg.dcomref,featureComDes.errordotIN)
plug(featureCom.errordot,taskComPD.errorDot)
taskComPD.controlGain.value = 40
taskComPD.setBeta(-1)
# --- TASK RIGHT FOOT
# Task right hand
taskRF=MetaTask6d('rf',dyn,'rf','right-ankle')
taskLF=MetaTask6d('lf',dyn,'lf','left-ankle')
plug(pg.rightfootref,taskRF.featureDes.position)
taskRF.task.controlGain.value = 5
plug(pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 5
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskComPD.name)
plug(sot.control,robot.control)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
# Set constraints on XY
pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
# The next command must be runned after a OpenHRP.inc ... ???
# Start the robot with a speed of 0.1 m/0.8 s.
pg.parseCmd(':HerdtOnline 0.1 0.0 0.0')
# You can now modifiy the speed of the robot using set pg.velocitydes [3]( x, y, yaw)
pg.velocitydes.value =(0.1,0.0,0.0)
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
from dynamic_graph.tracer_real_time import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
#tr.add(dyn.name+'.ffposition','ff')
tr.add(taskRF.featureDes.name+'.position','refr')
tr.add(taskLF.featureDes.name+'.position','refl')
matlab/sot/ddname.m 0 → 100755
View file @ 60cf95b2
function [e] = ddname
%e='C:\nmansard\code\VS_SAI-II\vsProject\xmlData\high-end-models-sentis\puma';
%e='C:\nmansard\code\VS_SAI-II\vsProject\xmlData\high-end-models-sentis\stanbot\non-fixed';
%e='C:\nmansard\code\VS_SAI-II\vsProject\xmlData\high-end-models-sentis\pa10';
%e='C:\nmansard\code\VS_SAI-II\vsProject\xmlData\high-end-models-sentis\pa10\xp1\xp1\c_freespace';
%e='C:\nmansard\code\VS_SAI-II\vsProject\xmlData\high-end-models-sentis\pa10\xp2\xp2\c_free';
e='/tmp/';
%e='/tmp/nmansard/exec';
%e='/home/nmansard/src/StackOfTasks/matlab/mesures_calib/onlinecomplet/';
%e='/home/nmansard/src/StackOfTasks/traces/exp1_seq';
matlab/sot/nmcolor.m 0 → 100755
View file @ 60cf95b2
function [h]=nmcolor(x,e,idx,name);
argc = nargin;
dim=size(e);
l=max(dim); n=min(dim);
if (argc==1)
e=x; [l n]=size(e); x=1:l;
argc = argc+1;
end;
if (argc==2)
idx=-1;
argc = argc+1;
end;
if (argc==3)
name = 'Plot ';
argc = argc+1;
end;
if (idx==-1)
idx=1:n;
end
nb_task=n;
for i=find(idx>n)
warning(sprintf('Plot index %d is not valid.',i));
end
idx=idx(find(idx<=n));
color = [ 0.75 0.75 0; 0 0 0.4; 1 0 0; 0 .5 0 ];
lwb = [ .5 2 .5 2 .5 2 .5 2 .5 2 .5 2];
lwb = [ .1 8 .5 2 .5 2 .5 2 .5 2 .5 2];
nbcolor=length(color(:,1));
color=[color;color];color=[color;color];color=[color;color];color=[color;color];
lw=[]; for i=lwb lw=[lw;ones(nbcolor,1)*i]; end
sty = ['- ';'- ';'- ';'- '];
sty = [sty;sty];sty = [sty;sty];sty = [sty;sty];sty = [sty;sty];
% gcf;
h=[];
seq=0;
for i=idx
idxs=mod(seq,length(sty))+1;
idxw=mod(seq,length(lw))+1;
idxc=mod(seq,length(color))+1;
hi=plot(x, e(:,i), 'color',color(idxc,:),'LineWidth',lw(idxw),'LineStyle',sty(idxs,:));
h=[h hi];
hold on;
seq=seq+1;
end;
%if separation
% ax =axis;
% hold on;
% st=stem(task*ax(3), '.k');
% set(st,'MarkerSize',1);
% st=stem(task*ax(4), '.k');
% set(st,'MarkerSize',1);
% hold off;
% axis(ax);
%end;
xlabel('Iterations');
ylabel('Values');
legende=[];
for i=idx
legende = [legende; sprintf('%s % 4d',name,i)];
end;
%legend(legende);
matlab/sot/p.m 0 → 100755
View file @ 60cf95b2
function [e t] = p (filename,idx,dirname);
argc = nargin;
if (argc==0)
filename='pos';
argc = argc+1;
end;
if (argc==1)
idx=-1;
argc = argc+1;
end;
if (argc==2)
dirname=ddname;
argc = argc+1;
end;
name=sprintf ('%s/%s.dat',dirname,filename);
e=load(name);
%size(e);
t=e(:,1);
e=e(:,2:end);
nmcolor(t,e,idx,'P ');
python/2013_coursens/p105_demo.py 0 → 100644
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
# ______________________________________________________________________________
# ******************************************************************************
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
import dynamic_graph.script_shortcuts
from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph.matlab import matlab
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from numpy import *
from dynamic_graph.sot.core.utils.history import History
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
robotName = 'hrp14small'
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
halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
q0=list(halfSittingConfig[robotName])
initialConfig[robotName]=tuple(q0)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,verbose=True)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot,dyn.com)
history.record()
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
#-----------------------------------------------------------------------------
#---- 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)
dyn.velocity.value = robotDim*(0.,)
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()
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
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)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(dyn)
# --- TASK AVOID
taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LARM_JOINT0')
taskShoulder.feature.frame('desired')
gotoNd(taskShoulder,(0,0,0),'010')
taskShoulder.task = TaskInequality('taskShoulderAvoid')
taskShoulder.task.add(taskShoulder.feature.name)
taskShoulder.task.referenceInf.value = (-10,) # Xmin, Ymin
taskShoulder.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskShoulder.task.dt.value=dt
taskShoulder.task.controlGain.value = 0.9
taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LARM_JOINT3')
taskElbow.feature.frame('desired')
gotoNd(taskElbow,(0,0,0),'010')
taskElbow.task = TaskInequality('taskElbowAvoid')
taskElbow.task.add(taskElbow.feature.name)
taskElbow.task.referenceInf.value = (-10,) # Xmin, Ymin
taskElbow.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskElbow.task.dt.value=dt
taskElbow.task.controlGain.value = 0.9
# --- TASK SUPPORT --------------------------------------------------
featureSupport = FeatureGeneric('featureSupport')
plug(dyn.com,featureSupport.errorIN)
plug(dyn.Jcom,featureSupport.jacobianIN)
taskSupport=TaskInequality('taskSupport')
taskSupport.add(featureSupport.name)
taskSupport.selec.value = '011'
taskSupport.referenceInf.value = (-0.08,-0.15,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax
taskSupport.dt.value=dt
# --- TASK SUPPORT SMALL --------------------------------------------
featureSupportSmall = FeatureGeneric('featureSupportSmall')
plug(dyn.com,featureSupportSmall.errorIN)
plug(dyn.Jcom,featureSupportSmall.jacobianIN)
taskSupportSmall=TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin
#askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax
taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax
taskSupportSmall.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
headMcam = dot(headMcam,rotate('x',10*pi/180))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- FOV ---
taskFoV = MetaTaskVisualPoint('FoV',dyn,'head','gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
taskFoV.task=TaskInequality('taskFoVineq')
taskFoV.task.add(taskFoV.feature.name)
[Xmax,Ymax]=[0.38,0.28]
taskFoV.task.referenceInf.value = (-Xmax,-Ymax) # Xmin, Ymin
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax
taskFoV.task.dt.value=dt
taskFoV.task.controlGain.value=0.01
taskFoV.featureDes.xy.value = (0,0)
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position,taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = dyn.lowerJl.value
taskJL.referenceSup.value = dyn.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
tr.add('dyn.com','com')
history = History(dyn,1)
# --- SHORTCUTS ----------------------------------------------------------------
qn = taskJL.normalizedPosition
@optionalparentheses
def pqn(details=True):
''' Display the normalized configuration vector. '''
qn.recompute(robot.state.time)
s = [ "{0:.1f}".format(v) for v in qn.value]
if details:
print("Rleg: "+" ".join(s[:6]))
print("Lleg: "+" ".join(s[6:12]))
print("Body: "+" ".join(s[12:16]))
print("Rarm: "+" ".join(s[16:23]))
print("Larm :"+" ".join(s[23:30]))
else:
print(" ".join(s[:30]))
def jlbound(t=None):
'''Display the velocity bound induced by the JL as a double-column matrix.'''
if t==None: t=robot.state.time
taskJL.task.recompute(t)
return matrix([ [float(x),float(y)] for x,y
in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
def p6d(R,t):
M=eye(4)
M[0:3,0:3]=R
M[0:3,3]=t
return M
def push(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in sot.toList():
sot.push(taskName)
if taskName!="taskposture" and "taskposture" in sot.toList():
sot.down("taskposture")
def pop(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in sot.toList(): sot.rm(taskName)
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
RAD=pi/180
comproj = [0.1,-0.95,1.6]
#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
class BallPosition:
def __init__(self,xyz=(0,-1.1,0.9)):
self.ball = xyz
self.prec = 0
self.t = 0
self.duration = 0
self.f = 0
self.xyz= self.ball
def move(self,xyz,duration=50):
self.prec = self.ball
self.ball = xyz
self.t = 0
self.duration = duration
self.changeTargets()
if duration>0:
self.f = lambda : self.moveDisplay()
attime(ALWAYS,self.f)
else:
self.moveDisplay()
def moveDisplay(self):
tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
self.t += 1
if self.t>self.duration and self.duration>0:
attime.stop(self.f)
self.xyz= xyz
def changeTargets(self):
gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
taskFoV.goto3D(self.ball)
b = BallPosition()
tr.add(taskJL.name+".normalizedPosition","qn")
tr.add('dyn.com','com')
tr.add(taskRH.task.name+'.error','erh')
tr.add(taskFoV.feature.name+'.error','fov')
tr.add(taskFoV.task.name+'.normalizedPosition','fovn')
tr.add(taskSupportSmall.name+".normalizedPosition",'comsmalln')
tr.add(taskSupport.name+".normalizedPosition",'comn')
robot.after.addSignal(taskJL.name+".normalizedPosition")
robot.after.addSignal(taskSupportSmall.name+".normalizedPosition")
robot.after.addSignal(taskFoV.task.name+".normalizedPosition")
robot.after.addSignal(taskFoV.feature.name+'.error')
robot.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
dyn.com.recompute(0)
taskCom.featureDes.errorIN.value = dyn.com.value
taskCom.task.controlGain.value = 10
ball = BallPosition((0,-1.1,0.9))
push(taskRH)
ball.move((0.5,-0.2,1.3),0)
next()
next()
next()
def config(ref=0):
if ref==0:
print '''Only the task RH'''
elif ref==1:
print '''Task RH + foot constraint, balance is kept'''
sot.addContact(contactRF)
sot.addContact(contactLF)
elif ref==2:
print '''Task RH + foot constraint, balance is lost'''
sot.addContact(contactRF)
sot.addContact(contactLF)
ball.move((0.15,-0.2,1.3),0)
elif ref==3:
print '''Task RH + foot constraint + COM='''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,-0.2,1.3),0)
elif ref==4:
print '''Task RH + foot constraint + COM= + JL'''
qu = list(dyn.upperJl.value)
qu[23]=-0.3
taskJL.referenceSup.value =tuple(qu)
push(taskJL)
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,-0.2,1.3),0)
elif ref==5:
print '''Task RH + foot constraint + COM<'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskSupport)
ball.move((0.15,-0.2,1.3),0)
elif ref==6:
print '''Task RH + foot constraint + COM= + SINGULARITE '''
print '''(press 4x i to reach it)'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.6,-0.2,1.3),0)
elif ref==7:
print '''Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.9,-0.2,1.3),0)
sot.down(taskRH.task.name)
sot.down(taskRH.task.name)
sot.damping.value = 0.1
else:
print '''Not a correct config.'''
return
go()
c=config
@optionalparentheses
def i():
xyz=ball.xyz
xyz[0] += 0.1
ball.move(vectorToTuple(xyz),30)
def menu(ref=0):
print '\n\n\n\n\n\n\n\n\n'
print '''0: Only the task RH'''
print '''1: Task RH + foot constraint, balance is kept'''
print '''2: Task RH + foot constraint, balance is lost'''
print '''3: Task RH + foot constraint + COM='''
print '''4: Task RH + foot constraint + COM= + JL'''
print '''5: Task RH + foot constraint + COM<'''
print '''6: Task RH + foot constraint + COM= + SINGULARITE '''
print '''7: Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
print ''
uinp = raw_input(' Config? >> ')
config(int(uinp))
menu()
python/2013_coursens/p105_demo_romeo.py 0 → 100644
View file @ 60cf95b2
# ______________________________________________________________________________
# ******************************************************************************
# ______________________________________________________________________________
# ******************************************************************************
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
import dynamic_graph.script_shortcuts
from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph.matlab import matlab
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from numpy import *
from dynamic_graph.sot.core.utils.history import History
from dynamic_graph.sot.dyninv.robot_specific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor,specificitiesName,jointRankName
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
robotName = 'romeo'
robotDim=robotDimension[robotName]
robot = RobotSimu('romeo')
robot.resize(robotDim)
dt=5e-3
from dynamic_graph.sot.dyninv.robot_specific import halfSittingConfig
x0=-0.00949035111398315034
y0=0
z0=0.64870185118253043
# halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+initialConfig[robotName][6:]
halfSittingConfig[robotName] = initialConfig[robotName]
q0=list(halfSittingConfig[robotName])
initialConfig[robotName]=tuple(q0)
robot.set( initialConfig[robotName] )
addRobotViewer(robot,small=True,small_extra=24,verbose=False)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot,dyn.com)
history.record()
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.,)
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
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.07,-0.02,0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(dyn)
# --- TASK AVOID
taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LShoulderPitch')
taskShoulder.feature.frame('desired')
gotoNd(taskShoulder,(0,0,0),'010')
taskShoulder.task = TaskInequality('taskShoulderAvoid')
taskShoulder.task.add(taskShoulder.feature.name)
taskShoulder.task.referenceInf.value = (-10,) # Xmin, Ymin
taskShoulder.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskShoulder.task.dt.value=dt
taskShoulder.task.controlGain.value = 0.9
taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LElbowYaw')
taskElbow.feature.frame('desired')
gotoNd(taskElbow,(0,0,0),'010')
taskElbow.task = TaskInequality('taskElbowAvoid')
taskElbow.task.add(taskElbow.feature.name)
taskElbow.task.referenceInf.value = (-10,) # Xmin, Ymin
taskElbow.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskElbow.task.dt.value=dt
taskElbow.task.controlGain.value = 0.9
# --- TASK SUPPORT --------------------------------------------------
featureSupport = FeatureGeneric('featureSupport')
plug(dyn.com,featureSupport.errorIN)
plug(dyn.Jcom,featureSupport.jacobianIN)
taskSupport=TaskInequality('taskSupport')
taskSupport.add(featureSupport.name)
taskSupport.selec.value = '011'
taskSupport.referenceInf.value = (-0.08,-0.15,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax
taskSupport.dt.value=dt
# --- TASK SUPPORT SMALL --------------------------------------------
featureSupportSmall = FeatureGeneric('featureSupportSmall')
plug(dyn.com,featureSupportSmall.errorIN)
plug(dyn.Jcom,featureSupportSmall.jacobianIN)
taskSupportSmall=TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin
#askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax
taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax
taskSupportSmall.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
headMcam = dot(headMcam,rotate('x',10*pi/180))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- FOV ---
taskFoV = MetaTaskVisualPoint('FoV',dyn,'head','gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
taskFoV.task=TaskInequality('taskFoVineq')
taskFoV.task.add(taskFoV.feature.name)
[Xmax,Ymax]=[0.38,0.28]
taskFoV.task.referenceInf.value = (-Xmax,-Ymax) # Xmin, Ymin
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax
taskFoV.task.dt.value=dt
taskFoV.task.controlGain.value=0.01
taskFoV.featureDes.xy.value = (0,0)
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position,taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = dyn.lowerJl.value
taskJL.referenceSup.value = dyn.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
tr.add('dyn.com','com')
history = History(dyn,1)
# --- SHORTCUTS ----------------------------------------------------------------
qn = taskJL.normalizedPosition
@optionalparentheses
def pqn(details=True):
''' Display the normalized configuration vector. '''
qn.recompute(robot.state.time)
s = [ "{0:.1f}".format(v) for v in qn.value]
if details:
print("Rleg: "+" ".join(s[:6]))
print("Lleg: "+" ".join(s[6:12]))
print("Body: "+" ".join(s[12:16]))
print("Rarm: "+" ".join(s[16:23]))
print("Larm :"+" ".join(s[23:30]))
else:
print(" ".join(s[:30]))
def jlbound(t=None):
'''Display the velocity bound induced by the JL as a double-column matrix.'''
if t==None: t=robot.state.time
taskJL.task.recompute(t)
return matrix([ [float(x),float(y)] for x,y
in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
def p6d(R,t):
M=eye(4)
M[0:3,0:3]=R
M[0:3,3]=t
return M
def push(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in sot.toList():
sot.push(taskName)
if taskName!="taskposture" and "taskposture" in sot.toList():
sot.down("taskposture")
def pop(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in sot.toList(): sot.rm(taskName)
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
RAD=pi/180
comproj = [0.1,-0.95,1.6]
#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
class BallPosition:
def __init__(self,xyz=(0,-1.1,0.9)):
self.ball = xyz
self.prec = 0
self.t = 0
self.duration = 0
self.f = 0
self.xyz= self.ball
def move(self,xyz,duration=50):
self.prec = self.ball
self.ball = xyz
self.t = 0
self.duration = duration
self.changeTargets()
if duration>0:
self.f = lambda : self.moveDisplay()
attime(ALWAYS,self.f)
else:
self.moveDisplay()
def moveDisplay(self):
tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
self.t += 1
if self.t>self.duration and self.duration>0:
attime.stop(self.f)
self.xyz= xyz
def changeTargets(self):
gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
taskFoV.goto3D(self.ball)
b = BallPosition()
tr.add(taskJL.name+".normalizedPosition","qn")
tr.add('dyn.com','com')
tr.add(taskRH.task.name+'.error','erh')
tr.add(taskFoV.feature.name+'.error','fov')
tr.add(taskFoV.task.name+'.normalizedPosition','fovn')
tr.add(taskSupportSmall.name+".normalizedPosition",'comsmalln')
tr.add(taskSupport.name+".normalizedPosition",'comn')
robot.after.addSignal(taskJL.name+".normalizedPosition")
robot.after.addSignal(taskSupportSmall.name+".normalizedPosition")
robot.after.addSignal(taskFoV.task.name+".normalizedPosition")
robot.after.addSignal(taskFoV.feature.name+'.error')
robot.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
dyn.com.recompute(0)
taskCom.featureDes.errorIN.value = dyn.com.value
taskCom.task.controlGain.value = 10
ball = BallPosition((0,-1.1,0.9))
push(taskRH)
ball.move((0.5,-0.2,1.0),0)
next()
next()
next()
def config(ref=0):
if ref==0:
print '''Only the task RH'''
elif ref==1:
print '''Task RH + foot constraint, balance is kept'''
sot.addContact(contactRF)
sot.addContact(contactLF)
elif ref==2:
print '''Task RH + foot constraint, balance is lost'''
sot.addContact(contactRF)
sot.addContact(contactLF)
ball.move((-0.15,-0.2,1.3),0)
print 'pouet'
elif ref==3:
print '''Task RH + foot constraint + COM='''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,0.1,1),0)
elif ref==4:
print '''Task RH + foot constraint + COM= + JL'''
qu = list(dyn.upperJl.value)
qu[19]=0
taskJL.referenceSup.value =tuple(qu)
push(taskJL)
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,0.1,1),0)
elif ref==5:
print '''Task RH + foot constraint + COM<'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskSupport)
ball.move((0.15,-0.2,1.3),0)
elif ref==6:
print '''Task RH + foot constraint + COM= + SINGULARITE '''
print '''(press 4x i to reach it)'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,-0.2,1.3),0)
elif ref==7:
print '''Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
sot.addContact(contactRF)
sot.addContact(contactLF)
push(taskCom)
ball.move((0.15,-0.2,1.3),0)
sot.down(taskRH.task.name)
sot.down(taskRH.task.name)
sot.damping.value = 0.1
else:
print '''Not a correct config.'''
return
go()
c=config
@optionalparentheses
def i():
xyz=ball.xyz
xyz[0] += 0.1
ball.move(vectorToTuple(xyz),30)
def menu(ref=0):
print '\n\n\n\n\n\n\n\n\n'
print '''0: Only the task RH'''
print '''1: Task RH + foot constraint, balance is kept'''
print '''2: Task RH + foot constraint, balance is lost'''
print '''3: Task RH + foot constraint + COM='''
print '''4: Task RH + foot constraint + COM= + JL'''
print '''5: Task RH + foot constraint + COM<'''
print '''6: Task RH + foot constraint + COM= + SINGULARITE '''
print '''7: Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
print ''
uinp = raw_input(' Config? >> ')
config(int(uinp))
menu()