From 4841a79edace7e47a311629cafbb4896a4077fe6 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 27 Jan 2011 19:27:30 +0100
Subject: [PATCH] IVIGIT.
---
CMakeLists.txt | 1 +
include/sot-dyninv/pseudo-robot-dynamic.h | 114 +++++++
src/dynamic_graph/sot/dyninv/__init__.py | 3 +
src/pseudo-robot-dynamic.cpp | 358 ++++++++++++++++++++++
4 files changed, 476 insertions(+)
create mode 100644 include/sot-dyninv/pseudo-robot-dynamic.h
create mode 100644 src/pseudo-robot-dynamic.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ccc4428..c224612 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -51,6 +51,7 @@ SET(libs
dynamic-integrator
solver-op-space
zmp-estimator
+ pseudo-robot-dynamic
)
LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
diff --git a/include/sot-dyninv/pseudo-robot-dynamic.h b/include/sot-dyninv/pseudo-robot-dynamic.h
new file mode 100644
index 0000000..a4ede95
--- /dev/null
+++ b/include/sot-dyninv/pseudo-robot-dynamic.h
@@ -0,0 +1,114 @@
+/*
+ * 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_PseudoRobotDynamic_H__
+#define __sot_dyninv_PseudoRobotDynamic_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (pseudo_robot_dynamic_EXPORTS)
+# define SOTPSEUDOROBOTDYNAMIC_EXPORT __declspec(dllexport)
+# else
+# define SOTPSEUDOROBOTDYNAMIC_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTPSEUDOROBOTDYNAMIC_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/* SOT */
+#include <sot-dyninv/signal-helper.h>
+#include <sot-dyninv/entity-helper.h>
+#include <sot-dyninv/dynamic-integrator.h>
+
+namespace sot {
+ namespace dyninv {
+
+ /* --------------------------------------------------------------------- */
+ /* --- CLASS ----------------------------------------------------------- */
+ /* --------------------------------------------------------------------- */
+
+ /* Inside this entity is an integrator, taking position, velocity and
+ * acceleration at time t, and computing pos and vel at time t+1. Around
+ * this integrator, a wrapper is made to make the stuff take the place of
+ * the OpenHRP kinematic entity. it thus take a "control" signal, to triger
+ * the computations, and possesses all the functionnalities of the OpenHRP
+ * entity. Plus, it has the signals to feed the OpenHRP entity that
+ * wrappes the simulator. All the computations are triggered by computing
+ * the "qdot" signal.
+ */
+
+ class SOTPSEUDOROBOTDYNAMIC_EXPORT PseudoRobotDynamic
+ :public DynamicIntegrator
+ ,public ::dynamicgraph::EntityHelper<PseudoRobotDynamic>
+ {
+
+ public: /* --- CONSTRUCTOR ---- */
+
+ PseudoRobotDynamic( const std::string & name );
+
+ public: /* --- ENTITY INHERITANCE --- */
+
+ static const std::string CLASS_NAME;
+ virtual void display( std::ostream& os ) const;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+ typedef ::dynamicgraph::EntityHelper<PseudoRobotDynamic>::EntityClassName
+ EntityClassName;
+
+ public: /* --- SIGNALS --- */
+
+ DECLARE_SIGNAL_IN( control,ml::Vector );
+ DECLARE_SIGNAL_OUT( qdot,ml::Vector );
+
+ DECLARE_SIGNAL(rotation,OUT,ml::Vector);
+ DECLARE_SIGNAL(translation,OUT,ml::Vector);
+ //sotSignal< ml::Vector,int > rotationSOUT;
+ //sotSignal< ml::Vector,int > translationSOUT;
+ ::dynamicgraph::SignalPtr< ml::Vector,int > stateSOUT;
+
+ public: /* --- SIGNALS --- */
+
+ void replaceSimulatorEntity( const std::string& formerName,
+ const bool& plug = false );
+ void setRoot( const ml::Matrix & M );
+
+ public: /* --- COMMAND --- */
+ template< typename T1 >
+ void forwardVoidCommandToSimu( const std::string& cmdName,
+ const T1& arg1 );
+ void addForward( const std::string& cmdName );
+
+ private:
+ ::dynamicgraph::Entity* formerOpenHRP;
+
+ }; // class PseudoRobotDynamic
+ } // namespace dyninv
+} // namespace sot
+
+
+
+#endif // #ifndef __sot_dyninv_PseudoRobotDynamic_H__
diff --git a/src/dynamic_graph/sot/dyninv/__init__.py b/src/dynamic_graph/sot/dyninv/__init__.py
index 47a35d2..484dda0 100755
--- a/src/dynamic_graph/sot/dyninv/__init__.py
+++ b/src/dynamic_graph/sot/dyninv/__init__.py
@@ -7,6 +7,9 @@ TaskDynPD('')
from dynamic_integrator import DynamicIntegrator
DynamicIntegrator('')
+from pseudo_robot_dynamic import PseudoRobotDynamic
+PseudoRobotDynamic('')
+
from solver_op_space import SolverOpSpace
SolverOpSpace('')
diff --git a/src/pseudo-robot-dynamic.cpp b/src/pseudo-robot-dynamic.cpp
new file mode 100644
index 0000000..93e8ca0
--- /dev/null
+++ b/src/pseudo-robot-dynamic.cpp
@@ -0,0 +1,358 @@
+/*
+ * 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/>.
+ */
+
+#include <sot-dyninv/pseudo-robot-dynamic.h>
+#include <sot-core/debug.h>
+
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/pool.h>
+
+#include <sot-dyninv/commands-helper.h>
+#include <sot-dyninv/mal-to-eigen.h>
+
+
+namespace sot
+{
+ namespace dyninv
+ {
+
+ namespace dg = ::dynamicgraph;
+ using namespace dg;
+
+ /* --- DG FACTORY ------------------------------------------------------- */
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PseudoRobotDynamic,"PseudoRobotDynamic");
+
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ /* --- CONSTRUCTION ----------------------------------------------------- */
+ PseudoRobotDynamic::
+ PseudoRobotDynamic( const std::string & name )
+ : DynamicIntegrator(name)
+
+ ,CONSTRUCT_SIGNAL_IN(control,ml::Vector)
+ ,CONSTRUCT_SIGNAL_OUT(qdot,ml::Vector,controlSIN)
+
+ ,CONSTRUCT_SIGNAL(rotation,OUT,ml::Vector)
+ ,CONSTRUCT_SIGNAL(translation,OUT,ml::Vector)
+ ,stateSOUT( &positionSOUT,getClassName()+"("+getName()+")::output(vector)::state" )
+
+ ,formerOpenHRP( NULL )
+ {
+ Entity::signalRegistration( controlSIN << qdotSOUT
+ << rotationSOUT << translationSOUT
+ << stateSOUT );
+
+ /* --- COMMANDS --- */
+ std::string doc
+ = docCommandVoid2("Replace in the pool a robot entity by this,"
+ "and modify the plugs (if 2de arg is true).",
+ "string (entity name)","bool (plug)");
+ addCommand("replace",
+ makeCommandVoid2(*this,&PseudoRobotDynamic::replaceSimulatorEntity,
+ doc));
+ doc = docCommandVoid1("Set the root position, and forward to the real simulator.",
+ "matrix homogeneous");
+ addCommand("setRoot",
+ makeCommandVoid1(*this,&PseudoRobotDynamic::setRoot,
+ doc));
+
+ doc = docCommandVoid1("Add to the current entity the command of the previous simu.",
+ "string (command name)");
+ addCommand("addForward",
+ makeCommandVoid1(*this,&PseudoRobotDynamic::addForward,
+ doc));
+
+ }
+
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+ /* --- SIGNALS ---------------------------------------------------------- */
+
+ /* Force the recomputation of the DynInt::controlSIN, and one step
+ * of the integrator. Then, export the resulting joint velocity,
+ * along with the 6D position of the free floating through signals
+ * rotationSOUT and translationSOUT.
+ */
+ ml::Vector& PseudoRobotDynamic::
+ qdotSOUT_function( ml::Vector& mlqdot, int time )
+ {
+ sotDEBUGIN(5);
+
+ controlSIN(time);
+ integrateFromSignals(time);
+
+ EIGEN_VECTOR_FROM_SIGNAL(v,velocity );
+ EIGEN_VECTOR_FROM_VECTOR( qdot,mlqdot,v.size()-6 );
+ qdot = v.tail(v.size()-6);
+
+ EIGEN_VECTOR_FROM_SIGNAL(p,position );
+ {
+ ml::Vector mlv3;
+ EIGEN_VECTOR_FROM_VECTOR( r,mlv3,3 );
+ r = p.segment(3,3);
+ rotationSOUT = mlv3;
+ r = p.head(3);
+ translationSOUT = mlv3;
+ }
+
+ sotDEBUGOUT(5);
+ return mlqdot;
+ }
+
+ /* --- TOOLS ------------------------------------------------------------- */
+ /* --- TOOLS ------------------------------------------------------------- */
+ /* --- TOOLS ------------------------------------------------------------- */
+
+ namespace PseudoRobotDynamic_Static
+ {
+ using namespace Eigen;
+
+ template< typename D1 >
+ Vector3d computeEulerFromRotationMatrix ( const MatrixBase<D1> & rotation )
+ {
+ Vector3d euler;
+
+ double rotationMatrix00 = rotation(0,0);
+ double rotationMatrix10 = rotation(1,0);
+ double rotationMatrix20 = rotation(2,0);
+ double rotationMatrix01 = rotation(0,1);
+ double rotationMatrix11 = rotation(1,1);
+ double rotationMatrix21 = rotation(2,1);
+ double rotationMatrix02 = rotation(0,2);
+ double rotationMatrix12 = rotation(1,2);
+ double rotationMatrix22 = rotation(2,2);
+
+ double cosTheta = sqrt(0.5 * ( rotationMatrix00*rotationMatrix00
+ + rotationMatrix10*rotationMatrix10
+ + rotationMatrix21*rotationMatrix21
+ + rotationMatrix22*rotationMatrix22 ));
+ double sinTheta = -rotationMatrix20;
+ euler[1] = atan2 (sinTheta, cosTheta);
+
+ double cosTheta_cosPhi = 0.5 * (rotationMatrix22 * rotationMatrix11
+ - rotationMatrix21 * rotationMatrix12);
+ double cosTheta_sinPhi = 0.5 * (rotationMatrix21 * rotationMatrix02
+ - rotationMatrix22 * rotationMatrix01);
+ double cosTheta_cosPsi = 0.5 * (rotationMatrix00 * rotationMatrix11
+ - rotationMatrix01 * rotationMatrix10);
+ double cosTheta_sinPsi = 0.5 * (rotationMatrix10 * rotationMatrix02
+ - rotationMatrix00 * rotationMatrix12);
+
+ //if cosTheta == 0
+ if (fabs(cosTheta) < 1e-9 )
+ {
+ if (sinTheta > 0.5) // sinTheta ~= 1
+ {
+ //phi_psi = phi - psi
+ double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
+ double psi = euler[2];
+
+ double phi = phi_psi + psi;
+ euler[0] = phi;
+ }
+ else //sinTheta ~= -1
+ {
+ //phi_psi = phi + psi
+ double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
+
+ double psi = euler[2];
+
+ double phi = phi_psi;
+ euler[0] = phi - psi;
+ }
+ }
+ else
+ {
+ double cosPsi = cosTheta_cosPsi / cosTheta;
+ double sinPsi = cosTheta_sinPsi / cosTheta;
+ euler[0] = atan2 (sinPsi, cosPsi);
+
+ double cosPhi = cosTheta_cosPhi / cosTheta;
+ double sinPhi = cosTheta_sinPhi / cosTheta;
+ euler[2] = atan2 (sinPhi, cosPhi);
+ }
+
+ return euler;
+ }
+
+ } // namespace PseudoRobotDynamic_Static
+
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+ /* --- COMMANDS ---------------------------------------------------------- */
+
+
+ void PseudoRobotDynamic::
+ replaceSimulatorEntity( const std::string& formerName, const bool & plug )
+ {
+ assert( formerOpenHRP == NULL );
+ formerOpenHRP = &dg::g_pool.getEntity( formerName );
+ dg::g_pool.deregisterEntity( formerName );
+ // former.name = formerName+".old";
+ dg::g_pool.registerEntity( formerName+"-old",formerOpenHRP );
+
+ entityDeregistration(); name = formerName; entityRegistration();
+
+ if( plug )
+ {
+ formerOpenHRP->getSignal("control").plug( & qdotSOUT );
+
+ try
+ {
+ formerOpenHRP->getSignal("attitudeIN").plug( & rotationSOUT );
+ formerOpenHRP->getSignal("translation").plug( & translationSOUT );
+ }
+ catch (...) {}
+
+ const ml::Vector& pos
+ = dynamic_cast< dg::Signal<ml::Vector,int>& >
+ ( formerOpenHRP->getSignal("state") ).accessCopy();
+ try
+ {
+ const ml::Vector& vel
+ = dynamic_cast< dg::Signal<ml::Vector,int>& >
+ ( formerOpenHRP->getSignal("velocity") ).accessCopy();
+ setState(pos,vel);
+ }
+ catch (... )
+ {
+ ml::Vector velzero( pos.size() ); velzero.setZero();
+ setState(pos,velzero);
+ }
+ }
+ }
+ void PseudoRobotDynamic::
+ setRoot( const ml::Matrix & mlM )
+ {
+ sotDEBUG(15) << "Set root with " << mlM << std::endl;
+ using namespace Eigen;
+ using PseudoRobotDynamic_Static::computeEulerFromRotationMatrix;
+
+ EIGEN_MATRIX_FROM_SIGNAL(M,mlM);
+ Vector3d r = computeEulerFromRotationMatrix( M.topLeftCorner(3,3) );
+ EIGEN_VECTOR_FROM_SIGNAL( q,position );
+ if( q.size()<6 )
+ {
+ throw; // TODO
+ }
+ q.head(3) = M.col(3).head(3);
+ q.segment(3,3) = r;
+
+ if( formerOpenHRP )
+ try
+ {
+ forwardVoidCommandToSimu( "setRoot",mlM );
+ }
+ catch(...) {}
+ }
+
+
+ /* --- FORWARD ---------------------------------------------------------- */
+
+ /* This function is a forward on the command on the former simu entity
+ * <formerOpenHRP>: when calling it, the parameters are send to the named
+ * command, that is executed.
+ */
+ template< typename T1 >
+ void PseudoRobotDynamic::
+ forwardVoidCommandToSimu( const std::string& cmdName,
+ const T1& arg1 )
+ {
+ /* Forward to previous entity. */
+ assert( formerOpenHRP );
+
+ using dg::command::Command;
+ using dg::command::Value;
+
+ Command* command = formerOpenHRP->getNewStyleCommand(cmdName);
+ std::vector<Value> valuesArg;
+ valuesArg.push_back( Value( arg1 ) );
+ command->setParameterValues(valuesArg);
+ command->execute();
+ }
+
+ /* Add the named command of the former simu entity <formerOpenHRP> to the
+ * commands of the entity.
+ */
+ void PseudoRobotDynamic::
+ addForward( const std::string& cmdName )
+ {
+ assert( formerOpenHRP );
+ addCommand(cmdName,formerOpenHRP->getNewStyleCommand(cmdName));
+ }
+
+
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+ /* --- ENTITY ----------------------------------------------------------- */
+
+ void PseudoRobotDynamic::
+ display( std::ostream& os ) const
+ {
+ os << "PseudoRobotDynamic "<<getName() << ". " << std::endl;
+ }
+
+ void PseudoRobotDynamic::
+ commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+ {
+ sotDEBUGIN(15);
+
+ if( cmdLine == "help" )
+ {
+ os << "PseudoRobotDynamic:" << std::endl
+ << " - replace <OpenHRP> [plug]" << std::endl;
+ }
+ else if( cmdLine == "replace" )
+ {
+ if( cmdArgs >> std::ws, cmdArgs.good() )
+ {
+ std::string repName; cmdArgs >> repName >> std::ws;
+ bool plug = cmdArgs.good();
+ replaceSimulatorEntity( repName,plug );
+ }
+ }
+ else if( cmdLine=="sbs" || cmdLine=="play" || cmdLine =="withForces"
+ || cmdLine == "periodicCall" || cmdLine == "periodicCallBefore"
+ || cmdLine == "periodicCallAfter" )
+ {
+ formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
+ }
+ else if( cmdLine=="root" )
+ {
+ if( cmdArgs >> std::ws, cmdArgs.good() )
+ {
+ ml::Matrix M; cmdArgs >> M; setRoot(M);
+ std::ostringstream osback;
+ osback << M.accessToMotherLib(); cmdArgs.str(osback.str());
+ formerOpenHRP ->commandLine( cmdLine,cmdArgs,os );
+ }
+ else
+ {
+ os << "TODO" << std::endl;
+ }
+ }
+ else
+ {
+ Entity::commandLine( cmdLine,cmdArgs,os );
+ }
+ sotDEBUGOUT(15);
+ }
+
+ } // namespace dyninv
+} // namespace sot
+
--
GitLab