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 (305)
205 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 1036 additions and 738 deletions
.gitignore
View file @ 60cf95b2
*build
*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
.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,31 +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
task-dyn-pd
dynamic-integrator
solver-op-space
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
......@@ -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 780d2e93...b58bd669
Subproject commit 780d2e93ee6e1765e2f2e34cb54c7c069a9781d8
Subproject commit b58bd669f6567662eefb8a410e8e40aeba4f1060
doc/CMakeLists.txt deleted 100644 → 0
View file @ 029ff75a
#
# 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
......
include/sot-dyninv/mal-to-eigen.h deleted 100644 → 0
View file @ 029ff75a
/*
* Copyright 2011, Nicolas Mansard, LAAS-CNRS
*
* This file is part of sot-dyninv.
* sot-dyninv 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-dyninv 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-dyninv. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_dyninv_mal_to_eigen_H__
#define __sot_dyninv_mal_to_eigen_H__
#include <Eigen/LU>
#include <soth/Algebra.hpp>
namespace Eigen
{
typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRXd;
}
#define EIGEN_MATRIX_FROM_SIGNAL(name,signal) \
Eigen::Map<Eigen::MatrixRXd> name \
( \
const_cast<double*>(signal.accessToMotherLib().data().begin()), \
signal.nbRows(), \
signal.nbCols() \
)
#define EIGEN_VECTOR_FROM_SIGNAL(name,signal) \
Eigen::Map<Eigen::VectorXd> name \
( \
signal.accessToMotherLib().data().begin(), \
signal.size() \
)
template< typename D >
inline void EIGEN_VECTOR_TO_VECTOR( const Eigen::MatrixBase<D>& in, ml::Vector & out )
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY( Eigen::MatrixBase<D> );
out.resize(in.size());
memcpy( out.accessToMotherLib().data().begin(),in.derived().data(),
in.size()*sizeof(double));
}
template< typename MB >
inline void EIGEN_ROWMAJOR_MATRIX_TO_MATRIX( const Eigen::MatrixBase<MB>& in,
ml::Matrix & out )
{
out.resize( in.rows(),in.cols() );
memcpy( out.accessToMotherLib().data().begin(),in.derived().data(),
in.cols()*in.rows()*sizeof(double));
}
template< typename MB >
inline void EIGEN_COLMAJOR_MATRIX_TO_MATRIX( const Eigen::MatrixBase<MB>& in,
ml::Matrix & out )
{
// TODO: find a better way for that!
out.resize( in.rows(),in.cols() );
for( int i=0;i<in.rows();++i )
for( int j=0;j<in.cols();++j )
out(i,j)=in(i,j);
}
#define EIGEN_MATRIX_FROM_MATRIX(eigName,mlName,r,c) \
mlName.resize(r,c); \
EIGEN_MATRIX_FROM_SIGNAL(eigName,mlName)
#define EIGEN_VECTOR_FROM_VECTOR(eigName,mlName,r) \
mlName.resize(r); \
EIGEN_VECTOR_FROM_SIGNAL(eigName,mlName)
#endif // __sot_dyninv_mal_to_eigen_H__
include/sot-dyninv/stack-template.h deleted 100644 → 0
View file @ 029ff75a
/*
* Copyright 2011, Nicolas Mansard, LAAS-CNRS
*
* This file is part of sot-dyninv.
* sot-dyninv 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-dyninv 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-dyninv. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_dyninv_StackTemplate_H__
#define __sot_dyninv_StackTemplate_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dynamic_interpretor_EXPORTS)
# define SOTSTACKTEMPLATE_EXPORT __declspec(dllexport)
# else
# define SOTSTACKTEMPLATE_EXPORT __declspec(dllimport)
# endif
#else
# define SOTSTACKTEMPLATE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot-dyninv/signal-helper.h>
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dg = dynamicgraph;
template< typename TaskGeneric >
class SOTSTACKTEMPLATE_EXPORT Stack
{
protected:
typedef std::vector<TaskGeneric*> Stack_t;
typedef typename Stack_t::iterator StackIterator_t;
typedef typename Stack_t::const_iterator StackConstIterator_t;
/*! \brief List of task (controllers) managed by the stack of tasks. */
Stack_t stack;
/*! \brief Number of joints to be used to compute the control law. */
int nbDofs;
public:
/*! \brief Number of joints by default. */
static const unsigned int NB_JOINTS_DEFAULT; // = 48;
public: /* --- Constructors --- */
Stack( void );
~Stack( void ) { /* TODO!! */ }
public: /* --- Modifiors --- */
/*! \name Methods to handle the stack.
@{
*/
/*! \brief Push the task in the stack.
It has a lowest priority than the previous ones.
If this is the first task, then it has the highest
priority. */
virtual void push( TaskGeneric& task );
/*! \brief Pop the task from the stack.
This method removes the task with the smallest
priority in the task. The other are projected
in the null-space of their predecessors. */
virtual TaskGeneric& pop( void );
/*! \brief This method allows to know if a task exists or not */
virtual bool exist( const TaskGeneric& task );
/*! \brief Remove a task regardless to its position in the stack.
It removes also the signals connected to the output signal of this stack.*/
virtual void remove( const TaskGeneric& task );
/*! \brief This method makes the task to swap with the task having the
immediate superior priority. */
virtual void up( const TaskGeneric& task );
/*! \brief This method makes the task to swap with the task having the
immediate inferior priority. */
virtual void down( const TaskGeneric& task );
/*! \brief Remove all the tasks from the stack. */
virtual void clear( void );
/*! Change the number of DOF, ie the field nbDofs. */
virtual void defineNbDof( const int& nbDof );
/*! @} */
dg::SignalBase<int>* tatata;
int titit;
typedef std::list< const dg::SignalBase<int>* > TaskDependancyList_t;
int totol;
/*! \brief Return the signal to be added/removed from the dependancy
* list of the control signal. */
virtual TaskDependancyList_t getTaskDependancyList( const TaskGeneric& task ) = 0;
virtual void addDependancy( const TaskDependancyList_t& depList ) = 0;
virtual void removeDependancy( const TaskDependancyList_t& depList ) = 0;
virtual void resetReady( void ) = 0;
public: /* --- DISPLAY --- */
/*! \name Methods to display the stack of tasks.
@{
*/
/*! Display the stack of tasks in text mode as a tree. */
virtual void display( std::ostream& os ) const;
/*! \brief Write the priority between tasks in the outstream os. */
virtual std::ostream &
writeGraph(const std::string & name, std::ostream & os) const;
/*! @} */
public: /* --- COMMANDS --- */
/*! @{ */
/* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
* as well as the EntityClassName. */
//#define ADD_COMMANDS_FOR_THE_STACK /// Code in the .t.cpp
/*! \brief This method deals with the command line.
The command given in argument is send to the stack of tasks by the shell.
The command understood by sot are:
<ul>
<li> Tasks
<ul>
<li> push <task> : Push a task in the stack (FILO).
<li> pop : Remove the task push in the stack.
<li> down <task> : Make the task have a higher priority, i.e.
swap with the task immediatly superior in priority.
<li> up <task> : Make the task have a lowest priority, i.e.
swap with the task immediatly inferior in priority.
<li> rm <task> : Remove the task from the stack.
</ul>
*/
virtual bool stackCommandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
void pushByTaskName( const std::string& taskName );
void removeByTaskName( const std::string& taskName );
void upByTaskName( const std::string& taskName );
void downByTaskName( const std::string& taskName );
/*! @} */
}; // class StackTemplate
template< typename TaskGeneric >
std::ostream&
operator<< ( std::ostream& os,const Stack<TaskGeneric>& sot )
{
sot.display(os);
return os;
}
} // namespace sot
#include <sot-dyninv/stack-template.t.cpp>
#endif // #ifndef __sot_dyninv_StackTemplate_H__
include/sot-dyninv/stack-template.t.cpp deleted 100644 → 0
View file @ 029ff75a
/*
* Copyright 2011, Nicolas Mansard, LAAS-CNRS
*
* This file is part of sot-dyninv.
* sot-dyninv 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-dyninv 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-dyninv. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_dyninv_StackTemplate_TCC__
#define __sot_dyninv_StackTemplate_TCC__
#include <dynamic-graph/pool.h>
#include <sot-core/debug.h>
#include <sot-core/task-abstract.h>
namespace sot
{
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< typename TaskGeneric >
const unsigned int Stack<TaskGeneric>::NB_JOINTS_DEFAULT = 46;
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< typename TaskGeneric >
Stack<TaskGeneric>::
Stack( void )
:stack()
,nbDofs( NB_JOINTS_DEFAULT )
{
}
/* --------------------------------------------------------------------- */
/* --- STACK MANIPULATION --- */
/* --------------------------------------------------------------------- */
template< typename TaskGeneric >
void Stack<TaskGeneric>::
push( TaskGeneric& task )
{
stack.push_back( &task );
addDependancy( getTaskDependancyList( task ) );
resetReady();
}
template< typename TaskGeneric >
TaskGeneric& Stack<TaskGeneric>::
pop( void )
{
TaskGeneric* res = stack.back();
stack.pop_back();
removeDependancy( getTaskDependancyList( *res ) );
resetReady();
return *res;
}
template< typename TaskGeneric >
bool Stack<TaskGeneric>::
exist( const TaskGeneric& key )
{
StackIterator_t it;
for ( it=stack.begin();stack.end()!=it;++it )
{
if( *it == &key ) { return true; }
}
return false;
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
remove( const TaskGeneric& key )
{
bool find =false;
StackIterator_t it;
for ( it=stack.begin();stack.end()!=it;++it )
{
if( *it == &key ) { find=true; break; }
}
if(! find ){ return; }
stack.erase( it );
removeDependancy( getTaskDependancyList( key ) );
resetReady();
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
up( const TaskGeneric& key )
{
bool find =false;
StackIterator_t it;
for ( it=stack.begin();stack.end()!=it;++it )
{
if( *it == &key ) { find=true; break; }
}
if( stack.begin()==it ) { return; }
if(! find ){ return; }
StackIterator_t pos=it; pos--;
TaskGeneric * task = *it;
stack.erase( it );
stack.insert( pos,task );
resetReady();
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
down( const TaskGeneric& key )
{
bool find =false;
StackIterator_t it;
for ( it=stack.begin();stack.end()!=it;++it )
{
if( *it == &key ) { find=true; break; }
}
if( stack.end()==it ) { return; }
if(! find ){ return; }
StackIterator_t pos=it; pos++;
TaskGeneric* task=*it;
stack.erase( it );
if( stack.end()==pos ){ stack.push_back(task); }
else
{
pos++;
stack.insert( pos,task );
}
resetReady();
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
clear( void )
{
for ( StackIterator_t it=stack.begin();stack.end()!=it;++it )
{
removeDependancy( getTaskDependancyList( **it ) );
}
stack.clear();
resetReady();
}
template< typename TaskGeneric >
void Stack<TaskGeneric>::
defineNbDof( const int& nbDof )
{
assert(nbDof >= 0);
nbDofs = nbDof;
resetReady();
}
/* --------------------------------------------------------------------- */
/* --- DISPLAY --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
template< typename TaskGeneric >
void Stack<TaskGeneric>::
display( std::ostream& os ) const
{
os << "+-----------------"<<std::endl<<"+ SOT "
<< std::endl<< "+-----------------"<<std::endl;
for ( StackConstIterator_t it=this->stack.begin();
this->stack.end()!=it;++it )
//BOOST_FOREACH( TaskGeneric* task, stack )
{
os << "| " << (*it)->getName() <<std::endl;
}
}
/* --------------------------------------------------------------------- */
/* --- COMMAND --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#define ACT_BY_TASK_NAME(ACT) \
template< typename TaskGeneric > \
void Stack<TaskGeneric>:: \
ACT##ByTaskName( const std::string& tname ) \
{ \
dg::Entity & taska = dg::g_pool.getEntity( tname ); \
TaskGeneric & task = dynamic_cast<TaskGeneric&>( taska ); \
ACT(task); \
}
ACT_BY_TASK_NAME(push)
ACT_BY_TASK_NAME(remove)
ACT_BY_TASK_NAME(up)
ACT_BY_TASK_NAME(down)
template< typename TaskGeneric >
bool Stack<TaskGeneric>::
stackCommandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUGIN(15);
if( cmdLine == "help")
{
os << "Stack of Tasks: "<<std::endl
<< " - push <task>"<< std::endl
<< " - pop"<< std::endl
<< " - down <task>"<< std::endl
<< " - up <task>"<< std::endl
<< " - rm <task>"<< std::endl
<< " - clear"<< std::endl
<< " - display"<< std::endl
<< " - nbDofs <nb> "<<std::endl;
}
else if( cmdLine == "clear")
{
clear();
}
else if( cmdLine == "push")
{
std::string tname; cmdArgs >> tname;
pushByTaskName(tname);
}
else if( cmdLine == "up")
{
std::string tname; cmdArgs >> tname;
upByTaskName(tname);
}
else if( cmdLine == "down")
{
std::string tname; cmdArgs >> tname;
downByTaskName(tname);
}
else if( cmdLine == "rm")
{
std::string tname; cmdArgs >> tname;
removeByTaskName(tname);
}
else if( cmdLine == "pop")
{
TaskGeneric& task = pop();
os << "Remove : "<< task << std::endl;
}
else if( cmdLine == "nbDofs")
{
cmdArgs>>std::ws;
if( cmdArgs.good() )
{
unsigned int nbDof;
cmdArgs >> nbDof;
defineNbDof(nbDof);
} else { os << "nbDofs = "<< nbDofs <<std::endl; }
}
else if( cmdLine == "display")
{
display(os);
}
else return false;
return true;
sotDEBUGOUT(15);
}
template< typename TaskGeneric >
std::ostream& Stack<TaskGeneric>::
writeGraph( const std::string & name, std::ostream& os ) const
{
StackConstIterator_t iter;
for( iter = stack.begin(); iter!=stack.end();++iter )
{
const TaskGeneric & task = **iter;
StackConstIterator_t nextiter =iter;
nextiter++;
if (nextiter!=stack.end())
{
TaskGeneric & nexttask = **nextiter;
os << "\t\t\t" << task.getName() << " -> " << nexttask.getName()
<< " [color=red]" << std::endl;
}
}
os << "\t\tsubgraph cluster_Tasks {" <<std::endl
<< "\t\t\tsubgraph cluster_" << name << " {" << std::endl
<< "\t\t\t\tcolor=lightsteelblue1; label=\"" << name
<< "\"; style=filled;" << std::endl;
for( iter = stack.begin(); iter!=stack.end();++iter )
{
const TaskGeneric & task = **iter;
os << "\t\t\t\t" << task.getName()
<< " [ label = \"" << task.getName() << "\" ," << std::endl
<< "\t\t\t\t fontcolor = black, color = black, "
<< "fillcolor = magenta, style=filled, shape=box ]" << std::endl;
}
os << "\t\t\t}" << std::endl;
os << "\t\t\t}" <<std::endl;
return os;
}
/* To use the macro, the typedef sot::Stack<TaskSpec> stack_t has to be set,
* as well as the EntityClassName. */
#define ADD_COMMANDS_FOR_THE_STACK \
addCommand("dispStack", \
makeCommandVerbose(*this,&stack_t::display, \
docCommandVerbose("Guess what?"))); \
addCommand("up", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const std::string&)) \
&stack_t::upByTaskName, \
docCommandVoid1("Up the named task.", \
"string (task name)"))); \
addCommand("down", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const std::string&)) \
&stack_t::downByTaskName, \
docCommandVoid1("Down the named task.", \
"string (task name)"))); \
addCommand("push", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const std::string&)) \
&stack_t::pushByTaskName, \
docCommandVoid1("Push the named task.", \
"string (task name)"))); \
addCommand("rm", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const std::string&)) \
&stack_t::removeByTaskName, \
docCommandVoid1("Remove the named task.", \
"string (task name)"))); \
addCommand("clear", \
makeCommandVoid0(*this, \
(void (EntityClassName::*)(void)) \
&stack_t::clear, \
docCommandVoid0("Clear the stack from all task."))); \
addCommand("pop", \
makeCommandVoid0(*this, \
(void (EntityClassName::*)(void)) \
&stack_t::pop, \
docCommandVoid0("Remove the last (lowest) task of the stack."))); \
\
addCommand("setSize", \
makeCommandVoid1(*this, \
(void (EntityClassName::*)(const int&)) \
&stack_t::defineNbDof, \
docCommandVoid1("??? TODO", \
"int"))); \
addCommand("getSize", \
makeDirectGetter(*this,&nbDofs, \
docDirectGetter("size","int")))
} // namespace sot
#endif // #ifndef __sot_dyninv_StackTemplate_TCC__
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()