Skip to content

GitLab

Commit 407bae86 authored by Thomas Moulard's avatar Thomas Moulard
Browse files

Update to new APIs.

parent 9eb9d4df
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 407bae86
......@@ -41,7 +41,7 @@ SETUP_PROJECT()
ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.8.0")
ADD_REQUIRED_DEPENDENCY("dynamicsJRLJapan >= 1.16.1")
ADD_REQUIRED_DEPENDENCY("hrp2Dynamics >= 1.3.0")
ADD_REQUIRED_DEPENDENCY("hrp2-dynamics >= 1.3.0")
ADD_REQUIRED_DEPENDENCY("hrp2-10-optimized >= 1.0")
ADD_REQUIRED_DEPENDENCY("hrp2_10")
......@@ -70,18 +70,9 @@ SET (dynamic-hrp2_plugins_dependencies dynamic)
# hrp2-10 dependencies.
LIST(APPEND libs dynamic-hrp2_10)
SET (dynamic-hrp2_10_plugins_dependencies dynamic)
SET(dynamic-hrp2_10_plugins_compile_flags
${_dynamicsJRLJapan_CFLAGS} ${_hrp210optimized_CFLAGS})
SET(dynamic-hrp2_10_plugins_link_flags
${_dynamicsJRLJapan_LDFLAGS} ${_hrp210optimized_LDFLAGS})
LIST(APPEND libs dynamic-hrp2_10_old)
SET (dynamic-hrp2_10_plugins_dependencies dynamic)
SET(dynamic-hrp2_10_old_plugins_compile_flags
${_dynamicsJRLJapan_CFLAGS} ${_hrp210optimized_CFLAGS})
SET(dynamic-hrp2_10_old_plugins_link_flags
${_dynamicsJRLJapan_LDFLAGS} ${_hrp210optimized_LDFLAGS})
ADD_SUBDIRECTORY(include)
ADD_SUBDIRECTORY(src)
......
include/sot-dynamic/angle-estimator.h
View file @ 407bae86
......@@ -39,7 +39,7 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
/* SOT */
......
include/sot-dynamic/dynamic-hrp2.h
View file @ 407bae86
......@@ -30,7 +30,7 @@
#include <sot-dynamic/dynamic.h>
/* JRL dynamic */
#include <hrp2Dynamics/hrp2OptHumanoidDynamicRobot.h>
#include <hrp2Dynamics/hrp2Opthumanoid-dynamic-robot.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
......
include/sot-dynamic/dynamic.h
View file @ 407bae86
......@@ -26,13 +26,13 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include "MatrixAbstractLayer/MatrixAbstractLayer.h"
#include <jrl/mal/boost.hh>
#include "jrl/mal/matrixabstractlayer.hh"
namespace ml = maal::boost;
/* JRL dynamic */
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <dynamicsJRLJapan/dynamicsJRLJapanFactory.h>
#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
#include <jrl/dynamics/dynamicsfactory.hh>
namespace djj = dynamicsJRLJapan;
/* SOT */
......
include/sot-dynamic/force-compensation.h
View file @ 407bae86
......@@ -26,7 +26,7 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
/* SOT */
......
include/sot-dynamic/integrator-force-exact.h
View file @ 407bae86
......@@ -26,7 +26,7 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
/* SOT */
......
include/sot-dynamic/integrator-force-rk4.h
View file @ 407bae86
......@@ -26,7 +26,7 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
/* SOT */
......
include/sot-dynamic/integrator-force.h
View file @ 407bae86
......@@ -26,7 +26,7 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
/* SOT */
......
include/sot-dynamic/mass-apparent.h
View file @ 407bae86
......@@ -26,7 +26,7 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
/* SOT */
......
include/sot-dynamic/matrix-inertia.h
View file @ 407bae86
......@@ -23,8 +23,8 @@
#include <ostream>
#include "MatrixAbstractLayer/boost.h"
#include "MatrixAbstractLayer/MatrixAbstractLayer.h"
#include "jrl/mal/boost.hh"
#include "jrl/mal/matrixabstractlayer.hh"
#include <sot-core/matrix-twist.h>
#include <sot-core/matrix-force.h>
......
include/sot-dynamic/waist-attitude-from-sensor.h
View file @ 407bae86
......@@ -26,7 +26,7 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
/* SOT */
......
include/sot-dynamic/zmpreffromcom.h
View file @ 407bae86
......@@ -26,7 +26,7 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include <MatrixAbstractLayer/boost.h>
#include <jrl/mal/boost.hh>
namespace ml = maal::boost;
/* SOT */
......
src/CMakeLists.txt
View file @ 407bae86
......@@ -21,20 +21,20 @@ ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
# provide path to libraries
LINK_DIRECTORIES(${DYNAMICSJRLJAPAN_LIBRARY_DIRS})
LINK_DIRECTORIES(${HRP2DYNAMICS_LIBRARY_DIRS})
LINK_DIRECTORIES(${HRP2_DYNAMICS_LIBRARY_DIRS})
LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
LINK_DIRECTORIES(${SOT_CORE_LIBRARY_DIRS})
LINK_DIRECTORIES(${JRL_MAL_LIBRARY_DIRS})
# Add compilation flags
ADD_DEFINITIONS(${DYNAMICSJRLJAPAN_CFLAGS})
ADD_DEFINITIONS(${HRP2DYNAMICS_CFLAGS})
ADD_DEFINITIONS(${HRP2_DYNAMICS_CFLAGS})
ADD_DEFINITIONS(${DYNAMIC_GRAPH_CFLAGS})
ADD_DEFINITIONS(${SOT_CORE_CFLAGS})
ADD_DEFINITIONS(${JRL_MAL_CFLAGS})
foreach(dlink ${DYNAMICSJRLJAPAN_LDFLAGS}
${HRP2DYNAMICS_LDFLAGS}
${HRP2_DYNAMICS_LDFLAGS}
${DYNAMIC_GRAPH_LDFLAGS}
${SOT_CORE_LDFLAGS}
${JRL_MAL_LDFLAGS}
......@@ -55,10 +55,10 @@ SET(libs
IF(${DYNAMICSJRLJAPAN_FOUND})
LIST(APPEND libs dynamic)
IF(${HRP2DYNAMICS_FOUND})
IF(${HRP2_DYNAMICS_FOUND})
LIST(APPEND libs dynamic-hrp2)
SET (dynamic-hrp2_plugins_dependencies dynamic)
ENDIF(${HRP2DYNAMICS_FOUND})
ENDIF(${HRP2_DYNAMICS_FOUND})
ENDIF(${DYNAMICSJRLJAPAN_FOUND})
IF(${_hrp210optimized_FOUND})
......@@ -100,7 +100,7 @@ FOREACH(lib ${libs})
#MESSAGE(FATAL_ERROR X ${DYNAMIC_GRAPH_LIBRARIES})
TARGET_LINK_LIBRARIES(${lib} ${SOT_CORE_LIBRARIES})
TARGET_LINK_LIBRARIES(${lib} ${JRL_MAL_LIBRARIES})
TARGET_LINK_LIBRARIES(${lib} ${HRP2DYNAMICS_LIBRARIES})
TARGET_LINK_LIBRARIES(${lib} ${HRP2_DYNAMICS_LIBRARIES})
TARGET_LINK_LIBRARIES(${lib} ${DYNAMICSJRLJAPAN_LIBRARIES})
TARGET_LINK_LIBRARIES(${lib} "${${lib}_additional_libs}")
ENDIF(UNIX)
......
src/angle-estimator.cpp
View file @ 407bae86
......@@ -22,7 +22,7 @@
#include <sot-core/debug.h>
#include <dynamic-graph/factory.h>
#include <MatrixAbstractLayer/MatrixAbstractLayer.h>
#include <jrl/mal/matrixabstractlayer.hh>
using namespace sot;
using namespace dynamicgraph;
......
src/dynamic-hrp2.cpp
View file @ 407bae86
......@@ -18,12 +18,12 @@
* with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
*/
#include <MatrixAbstractLayer/MatrixAbstractLayer.h>
#include <jrl/mal/matrixabstractlayer.hh>
#include <sot-dynamic/dynamic-hrp2.h>
#include <sot-core/debug.h>
#include <robotDynamics/jrlRobotDynamicsObjectConstructor.h>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
#include <dynamic-graph/factory.h>
......
src/dynamic-hrp2_10.cpp
View file @ 407bae86
......@@ -21,7 +21,7 @@
#include <sot-dynamic/dynamic-hrp2_10.h>
#include <sot-core/debug.h>
#include <robotDynamics/jrlRobotDynamicsObjectConstructor.h>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
#include <dynamic-graph/factory.h>
using namespace dynamicgraph;
......
src/dynamic-hrp2_10_old.cpp
View file @ 407bae86
......@@ -21,7 +21,7 @@
#include <sot-dynamic/dynamic-hrp2_10_old.h>
#include <sot-core/debug.h>
#include <robotDynamics/jrlRobotDynamicsObjectConstructor.h>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
#include <dynamic-graph/factory.h>
using namespace dynamicgraph;
......
src/dynamic.cpp
View file @ 407bae86
......@@ -18,13 +18,13 @@
* with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
*/
#include <MatrixAbstractLayer/MatrixAbstractLayer.h>
#include <jrl/mal/matrixabstractlayer.hh>
#include <sot-dynamic/dynamic.h>
#include <sot-core/debug.h>
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <robotDynamics/jrlRobotDynamicsObjectConstructor.h>
#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
#include <dynamic-graph/factory.h>
......@@ -36,7 +36,7 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Dynamic,"Dynamic");
using namespace std;
Dynamic::
Dynamic( const std::string & name, bool build )
Dynamic( const std::string & name, bool build )
:Entity(name)
,m_HDR(NULL)
......@@ -55,48 +55,48 @@ Dynamic( const std::string & name, bool build )
,freeFlyerAccelerationSIN(NULL,"sotDynamic("+name+")::input(vector)::ffacceleration")
,firstSINTERN( boost::bind(&Dynamic::initNewtonEuler,this,_1,_2),
sotNOSIGNAL,"sotDynamic("+name+")::intern(dummy)::init" )
sotNOSIGNAL,"sotDynamic("+name+")::intern(dummy)::init" )
,newtonEulerSINTERN( boost::bind(&Dynamic::computeNewtonEuler,this,_1,_2),
firstSINTERN<<jointPositionSIN<<freeFlyerPositionSIN
<<jointVelocitySIN<<freeFlyerVelocitySIN
<<jointAccelerationSIN<<freeFlyerAccelerationSIN,
"sotDynamic("+name+")::intern(dummy)::newtoneuleur" )
"sotDynamic("+name+")::intern(dummy)::newtoneuleur" )
,zmpSOUT( boost::bind(&Dynamic::computeZmp,this,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(vector)::zmp" )
"sotDynamic("+name+")::output(vector)::zmp" )
,JcomSOUT( boost::bind(&Dynamic::computeJcom,this,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(matrix)::Jcom" )
"sotDynamic("+name+")::output(matrix)::Jcom" )
,comSOUT( boost::bind(&Dynamic::computeCom,this,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(vector)::com" )
"sotDynamic("+name+")::output(vector)::com" )
,inertiaSOUT( boost::bind(&Dynamic::computeInertia,this,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(matrix)::inertia" )
"sotDynamic("+name+")::output(matrix)::inertia" )
,footHeightSOUT( boost::bind(&Dynamic::computeFootHeight,this,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(double)::footHeight" )
"sotDynamic("+name+")::output(double)::footHeight" )
,upperJlSOUT( boost::bind(&Dynamic::getUpperJointLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamic("+name+")::output(vector)::upperJl" )
"sotDynamic("+name+")::output(vector)::upperJl" )
,lowerJlSOUT( boost::bind(&Dynamic::getLowerJointLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamic("+name+")::output(vector)::lowerJl" )
"sotDynamic("+name+")::output(vector)::lowerJl" )
,inertiaRotorSOUT( "sotDynamic("+name+")::output(matrix)::inertiaRotor" )
,gearRatioSOUT( "sotDynamic("+name+")::output(matrix)::gearRatio" )
,inertiaRealSOUT( boost::bind(&Dynamic::computeInertiaReal,this,_1,_2),
inertiaSOUT << gearRatioSOUT << inertiaRotorSOUT,
"sotDynamic("+name+")::output(matrix)::inertiaReal" )
"sotDynamic("+name+")::output(matrix)::inertiaReal" )
,MomentaSOUT( boost::bind(&Dynamic::computeMomenta,this,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(vector)::momenta" )
"sotDynamic("+name+")::output(vector)::momenta" )
,AngularMomentumSOUT( boost::bind(&Dynamic::computeAngularMomentum,this,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(vector)::angularmomentum" )
"sotDynamic("+name+")::output(vector)::angularmomentum" )
{
sotDEBUGIN(5);
......@@ -208,10 +208,10 @@ createJacobianSignal( const std::string& signame,const unsigned int& bodyRank )
"(rank=%d, while creating J signal).",
bodyRank );
}
CjrlJoint * aJoint = aVec[bodyRank];
CjrlJoint * aJoint = aVec[bodyRank];
dg::SignalTimeDependent< ml::Matrix,int > * sig
dg::SignalTimeDependent< ml::Matrix,int > * sig
= new dg::SignalTimeDependent< ml::Matrix,int >
( boost::bind(&Dynamic::computeGenericJacobian,this,aJoint,_1,_2),
newtonEulerSINTERN,
......@@ -235,10 +235,10 @@ createEndeffJacobianSignal( const std::string& signame,const unsigned int& bodyR
"(rank=%d, while creating J signal).",
bodyRank );
}
CjrlJoint * aJoint = aVec[bodyRank];
CjrlJoint * aJoint = aVec[bodyRank];
dg::SignalTimeDependent< ml::Matrix,int > * sig
dg::SignalTimeDependent< ml::Matrix,int > * sig
= new dg::SignalTimeDependent< ml::Matrix,int >
( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,aJoint,_1,_2),
newtonEulerSINTERN,
......@@ -264,7 +264,7 @@ destroyJacobianSignal( const std::string& signame )
}
if(! deletable )
{
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
"Cannot destroy signal",
" (while trying to remove generic jac. signal <%s>).",
......@@ -294,9 +294,9 @@ createPositionSignal( const std::string& signame,const unsigned int& bodyRank )
"(rank=%d, while creating position signal).",
bodyRank );
}
CjrlJoint * aJoint = aVec[bodyRank];
CjrlJoint * aJoint = aVec[bodyRank];
dg::SignalTimeDependent< MatrixHomogeneous,int > * sig
dg::SignalTimeDependent< MatrixHomogeneous,int > * sig
= new dg::SignalTimeDependent< MatrixHomogeneous,int >
( boost::bind(&Dynamic::computeGenericPosition,this,aJoint,_1,_2),
newtonEulerSINTERN,
......@@ -323,7 +323,7 @@ destroyPositionSignal( const std::string& signame )
}
if(! deletable )
{
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
"Cannot destroy signal",
" (while trying to remove generic pos. signal <%s>).",
......@@ -411,9 +411,9 @@ createAccelerationSignal( const std::string& signame,const unsigned int& bodyRan
"(rank=%d, while creating acceleration signal).",
bodyRank );
}
CjrlJoint * aJoint = aVec[bodyRank];
CjrlJoint * aJoint = aVec[bodyRank];
dg::SignalTimeDependent< ml::Vector,int > * sig
dg::SignalTimeDependent< ml::Vector,int > * sig
= new dg::SignalTimeDependent< ml::Vector,int >
( boost::bind(&Dynamic::computeGenericAcceleration,this,aJoint,_1,_2),
newtonEulerSINTERN,
......@@ -440,7 +440,7 @@ destroyAccelerationSignal( const std::string& signame )
}
if(! deletable )
{
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
"Cannot destroy signal",
" (while trying to remove generic acc signal <%s>).",
......@@ -455,15 +455,15 @@ destroyAccelerationSignal( const std::string& signame )
/* --- COMPUTE -------------------------------------------------------------- */
/* --- COMPUTE -------------------------------------------------------------- */
#include <MatrixAbstractLayer/boostspecific.h>
#include <jrl/mal/boostspecific.hh>
static void MAAL1_V3d_to_MAAL2( const vector3d& source,
ml::Vector & res )
{
{
sotDEBUG(5) << source <<endl;
res(0) = source[0];
res(1) = source[1];
res(2) = source[2];
res(0) = source[0];
res(1) = source[1];
res(2) = source[2];
}
ml::Matrix& Dynamic::
......@@ -521,7 +521,7 @@ computeGenericPosition( CjrlJoint * aJoint,MatrixHomogeneous& res,int time )
res.resize(4,4);
for( int i=0;i<4;++i )
for( int j=0;j<4;++j )
for( int j=0;j<4;++j )
res(i,j) = MAL_S4x4_MATRIX_ACCESS_I_J(m4,i,j);
// aJoint->computeJacobianJointWrtConfig();
......@@ -552,7 +552,7 @@ computeGenericPosition( CjrlJoint * aJoint,MatrixHomogeneous& res,int time )
MAL_S4x4_MATRIX_ACCESS_I_J(invrot,i,j);
//end of the adaptation
sotDEBUGOUT(25);
return res;
}
......@@ -662,7 +662,6 @@ computeJcom( ml::Matrix& Jcom,int time )
{
sotDEBUGIN(25);
newtonEulerSINTERN(time);
m_HDR->computeJacobianCenterOfMass();
Jcom.initFromMotherLib(m_HDR->jacobianCenterOfMass());
sotDEBUGOUT(25);
return Jcom;
......@@ -692,34 +691,34 @@ computeInertia( ml::Matrix& A,int time )
{
for( unsigned int i=0;i<18;++i )
for( unsigned int j=0;j<36;++j )
if( i==j ) A(i,i)=1;
if( i==j ) A(i,i)=1;
else { A(i,j)=A(j,i)=0; }
for( unsigned int i=20;i<22;++i )
for( unsigned int j=0;j<36;++j )
if( i==j ) A(i,i)=1;
if( i==j ) A(i,i)=1;
else { A(i,j)=A(j,i)=0; }
for( unsigned int i=28;i<36;++i )
for( unsigned int j=0;j<36;++j )
if( i==j ) A(i,i)=1;
if( i==j ) A(i,i)=1;
else { A(i,j)=A(j,i)=0; }
}
else if( 2==debugInertia )
{
for( unsigned int i=0;i<18;++i )
for( unsigned int j=0;j<36;++j )
if( i==j ) A(i,i)=1;
if( i==j ) A(i,i)=1;
else { A(i,j)=A(j,i)=0; }
for( unsigned int i=20;i<22;++i )
for( unsigned int j=0;j<36;++j )
if( i==j ) A(i,i)=1;
if( i==j ) A(i,i)=1;
else { A(i,j)=A(j,i)=0; }
for( unsigned int i=28;i<29;++i )
for( unsigned int j=0;j<36;++j )
if( i==j ) A(i,i)=1;
if( i==j ) A(i,i)=1;
else { A(i,j)=A(j,i)=0; }
for( unsigned int i=35;i<36;++i )
for( unsigned int j=0;j<36;++j )
if( i==j ) A(i,i)=1;
if( i==j ) A(i,i)=1;
else { A(i,j)=A(j,i)=0; }
}
......@@ -738,7 +737,7 @@ computeInertiaReal( ml::Matrix& res,int time )
res = A;
for( unsigned int i=0;i<gearRatio.size();++i )
res(i,i) += (gearRatio(i)*gearRatio(i)*inertiaRotor(i));
res(i,i) += (gearRatio(i)*gearRatio(i)*inertiaRotor(i));
sotDEBUGOUT(25);
return res;
......@@ -769,15 +768,15 @@ jacobiansSOUT( const std::string& name )
{
SignalBase<int> & sigabs = Entity::getSignal(name);
try {
dg::SignalTimeDependent<ml::Matrix,int>& res
try {
dg::SignalTimeDependent<ml::Matrix,int>& res
= dynamic_cast< dg::SignalTimeDependent<ml::Matrix,int>& >( sigabs );
return res;
} catch( std::bad_cast e ) {
SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
"Impossible cast.",
" (while getting signal <%s> of type matrix.",
name.c_str());
name.c_str());
}
}
dg::SignalTimeDependent<MatrixHomogeneous,int>& Dynamic::
......@@ -785,15 +784,15 @@ positionsSOUT( const std::string& name )
{
SignalBase<int> & sigabs = Entity::getSignal(name);
try {
dg::SignalTimeDependent<MatrixHomogeneous,int>& res
try {
dg::SignalTimeDependent<MatrixHomogeneous,int>& res
= dynamic_cast< dg::SignalTimeDependent<MatrixHomogeneous,int>& >( sigabs );
return res;
} catch( std::bad_cast e ) {
SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
"Impossible cast.",
" (while getting signal <%s> of type matrixHomo.",
name.c_str());
name.c_str());
}
}
......@@ -818,15 +817,15 @@ accelerationsSOUT( const std::string& name )
{
SignalBase<int> & sigabs = Entity::getSignal(name);
try {
dg::SignalTimeDependent<ml::Vector,int>& res
try {
dg::SignalTimeDependent<ml::Vector,int>& res
= dynamic_cast< dg::SignalTimeDependent<ml::Vector,int>& >( sigabs );
return res;
} catch( std::bad_cast e ) {
SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
"Impossible cast.",
" (while getting signal <%s> of type Vector.",
name.c_str());
name.c_str());
}
}
......@@ -852,15 +851,15 @@ computeNewtonEuler( int& dummy,int time )
if( freeFlyerVelocitySIN )
{
const ml::Vector& ffvel = freeFlyerVelocitySIN(time);
for( int i=0;i<6;++i ) vel(i) = ffvel(i);
for( int i=0;i<6;++i ) vel(i) = ffvel(i);
}
if( freeFlyerAccelerationSIN )
{
const ml::Vector& ffacc = freeFlyerAccelerationSIN(time);
for( int i=0;i<6;++i ) acc(i) = ffacc(i);
for( int i=0;i<6;++i ) acc(i) = ffacc(i);
}
if(! m_HDR->currentConfiguration(pos.accessToMotherLib()))
{
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
"Position vector size uncorrect",
" (Vector size is %d, should be %d).",
......@@ -869,7 +868,7 @@ computeNewtonEuler( int& dummy,int time )
if(! m_HDR->currentVelocity(vel.accessToMotherLib()) )
{
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
"Velocity vector size uncorrect",
" (Vector size is %d, should be %d).",
......@@ -877,7 +876,7 @@ computeNewtonEuler( int& dummy,int time )
}
if(! m_HDR->currentAcceleration(acc.accessToMotherLib()) )
{
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
"Acceleration vector size uncorrect",
" (Vector size is %d, should be %d).",
......@@ -885,7 +884,7 @@ computeNewtonEuler( int& dummy,int time )
}
m_HDR->computeForwardKinematics();
sotDEBUG(1)<< "pos = " <<pos <<endl;
sotDEBUG(1)<< "vel = " <<vel <<endl;
sotDEBUG(1)<< "acc = " <<acc <<endl;
......@@ -949,10 +948,10 @@ commandLine( const std::string& cmdLine,
sotDEBUG(25) << "# In { Cmd " << cmdLine <<endl;
std::string filename;
if( cmdLine == "debugInertia" )
{
cmdArgs>>ws; if(cmdArgs.good())
{
std::string arg; cmdArgs >> arg;
{
cmdArgs>>ws; if(cmdArgs.good())
{
std::string arg; cmdArgs >> arg;
if( (arg=="true")||(arg=="1") )
{ debugInertia = 1; }
else if( (arg=="2")||(arg=="grip") )
......@@ -972,17 +971,17 @@ commandLine( const std::string& cmdLine,
{ cmdArgs>>filename; setXmlRankFile( filename ); }
else if( cmdLine == "setFiles" )
{
cmdArgs>>filename; setVrmlDirectory( filename );
cmdArgs>>filename; setVrmlMainFile( filename );