diff --git a/cmake_metapod_configure.cmake b/cmake_metapod_configure.cmake
index 797c81c2167888064c9e7b5e5156a101a40d3df7..94b8c76518f7fb00276adaa2f97828d3fce27f7b 100644
--- a/cmake_metapod_configure.cmake
+++ b/cmake_metapod_configure.cmake
@@ -69,9 +69,9 @@ FUNCTION(ADD_SAMPLEURDFMODEL name)
ENDIF()
SET(_libname "metapod_${name}")
SET(_data_path "$ENV{ROS_WORKSPACE}/install/share/metapod/data/${name}")
- SET(_urdf_file "$ENV{ROS_WORKSPACE}/stacks/hrp2/hrp2_14_description/urdf/hrp2_14_reduced.urdf")
+ SET(_urdf_file "${_data_path}/${name}.urdf")
SET(_config_file "${_data_path}/${name}.config")
- SET(_license_file "${_data_path}/hrp2_14_license_file.txt")
+ SET(_license_file "${_data_path}/${name}_license_file.txt")
SET(_model_dir "$ENV{ROS_WORKSPACE}/install/include/metapod/models/${name}")
SET(METAPODFROMURDF_EXECUTABLE "$ENV{ROS_WORKSPACE}/install/bin/metapodfromurdf")
diff --git a/include/jrl/walkgen/pgtypes.hh b/include/jrl/walkgen/pgtypes.hh
index b54abd7eeac07cdccd8aee2b5b159c00514d7cb4..02e03723eebca90acd63f82c70eab2d64aa41608 100644
--- a/include/jrl/walkgen/pgtypes.hh
+++ b/include/jrl/walkgen/pgtypes.hh
@@ -166,6 +166,35 @@ namespace PatternGeneratorJRL
return os;
}
+ /// Structure to store the absolute foot position.
+ struct HandAbsolutePosition_t
+ {
+ /*! x, y, z in meters, theta in DEGREES. */
+ double x,y,z, theta, omega, omega2;
+ /*! Speed of the foot. */
+ double dx,dy,dz, dtheta, domega, domega2;
+ /*! Acceleration of the foot. */
+ double ddx,ddy,ddz, ddtheta, ddomega, ddomega2;
+ /*! Jerk of the hand. */
+ double dddx,dddy,dddz, dddtheta, dddomega, dddomega2;
+ /*! Time at which this position should be reached. */
+ double time;
+ /*! -1 : contact
+ * 1 : no contact
+ */
+ int stepType;
+ };
+ typedef struct HandAbsolutePosition_t HandAbsolutePosition;
+
+ inline std::ostream & operator<<(std::ostream & os, const HandAbsolutePosition& hap)
+ {
+ os << "x " << hap.x << " y " << hap.y << " z " << hap.z << " theta " << hap.theta << " omega " << hap.omega << " omega2 " << hap.omega2 << std::endl;
+ os << "dx " << hap.dx << " dy " << hap.dy << " dz " << hap.dz << " dtheta " << hap.dtheta << " domega " << hap.domega << " domega2 " << hap.domega2 << std::endl;
+ os << "ddx " << hap.ddx << " ddy " << hap.ddy << " ddz " << hap.ddz << " ddtheta " << hap.ddtheta << " ddomega " << hap.ddomega << " ddomega2 " << hap.ddomega2 << std::endl;
+ os << "time " << hap.time << " stepType " << hap.stepType;
+ return os;
+ }
+
// Linear constraint.
struct LinearConstraintInequality_s
{
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ee39ce53b6975c19d1eb34f71b443e9a2e38d0d5..c7c7901aa7eaa4571dc1b08c978111ca85025fc7 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -31,7 +31,78 @@ IF(USE_LSSOL)
ADD_DEFINITIONS("/DLSSOL_FOUND")
ENDIF(USE_LSSOL)
+SET(INCLUDES
+ PreviewControl/rigid-body.hh
+ PreviewControl/OptimalControllerSolver.hh
+ PreviewControl/rigid-body-system.hh
+ PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh
+ PreviewControl/SupportFSM.hh
+ PreviewControl/LinearizedInvertedPendulum2D.hh
+ PreviewControl/PreviewControl.hh
+ PreviewControl/SupportFSM_backup.hh
+ FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.hh
+ FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
+ FootTrajectoryGeneration/FootTrajectoryGenerationStandard.hh
+ FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
+ Debug.hh
+ SimplePluginManager.hh
+ privatepgtypes.hh
+ MultiContactRefTrajectoryGeneration/MultiContactHirukawa.hh
+ patterngeneratorinterfaceprivate.hh
+ Mathematics/FootConstraintsAsLinearSystem.hh
+ Mathematics/Polynome.hh
+ Mathematics/ConvexHull.hh
+ Mathematics/Bsplines.hh
+ Mathematics/StepOverPolynome.hh
+ Mathematics/AnalyticalZMPCOGTrajectory.hh
+ Mathematics/qld.hh
+ Mathematics/PLDPSolver.hh
+ Mathematics/FootHalfSize.hh
+ Mathematics/relative-feet-inequalities.hh
+ Mathematics/intermediate-qp-matrices.hh
+ Mathematics/PolynomeFoot.hh
+ Mathematics/PLDPSolverHerdt.hh
+ Mathematics/OptCholesky.hh
+ StepStackHandler.hh
+ configJRLWPG.hh
+ Clock.hh
+ GlobalStrategyManagers/CoMAndFootOnlyStrategy.hh
+ GlobalStrategyManagers/GlobalStrategyManager.hh
+ GlobalStrategyManagers/DoubleStagePreviewControlStrategy.hh
+ ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.hh
+ ZMPRefTrajectoryGeneration/DynamicFilter.hh
+ ZMPRefTrajectoryGeneration/ZMPConstrainedQPFastFormulation.hh
+ ZMPRefTrajectoryGeneration/qp-problem.hh
+ ZMPRefTrajectoryGeneration/ZMPDiscretization.hh
+ ZMPRefTrajectoryGeneration/OrientationsPreview.hh
+ ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
+ ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.hh
+ ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+ ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
+ ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+ ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh
+ ZMPRefTrajectoryGeneration/problem-vel-ref.hh
+ ZMPRefTrajectoryGeneration/ZMPQPWithConstraint.hh
+ SimplePlugin.hh
+ portability/gettimeofday.hh
+ portability/bzero.hh
+ MotionGeneration/ComAndFootRealizationByGeometry.hh
+ MotionGeneration/StepOverPlanner.hh
+ MotionGeneration/WaistHeightVariation.hh
+ MotionGeneration/ComAndFootRealization.hh
+ MotionGeneration/GenerateMotionFromKineoWorks.hh
+ MotionGeneration/UpperBodyMotion.hh
+ MotionGeneration/CollisionDetector.hh
+ ../tests/CommonTools.hh
+ ../tests/ClockCPUTime.hh
+ ../tests/TestObject.hh
+ ../doc/additionalHeader/modules.hh
+ ../include/jrl/walkgen/pgtypes.hh
+ ../include/jrl/walkgen/patterngeneratorinterface.hh
+)
+
SET(SOURCES
+ ${INCLUDES}
FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
@@ -75,6 +146,7 @@ SET(SOURCES
ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+ MultiContactRefTrajectoryGeneration/MultiContactHirukawa.cc
MotionGeneration/StepOverPlanner.cpp
MotionGeneration/CollisionDetector.cpp
MotionGeneration/WaistHeightVariation.cpp
diff --git a/src/MultiContactRefTrajectoryGeneration/MultiContactHirukawa.cc b/src/MultiContactRefTrajectoryGeneration/MultiContactHirukawa.cc
new file mode 100644
index 0000000000000000000000000000000000000000..841da1a512341fba4a87110b8333679460198b32
--- /dev/null
+++ b/src/MultiContactRefTrajectoryGeneration/MultiContactHirukawa.cc
@@ -0,0 +1,142 @@
+#include "MultiContactHirukawa.hh"
+//#define VERBOSE
+
+using namespace std ;
+using namespace PatternGeneratorJRL ;
+MultiContactHirukawa::MultiContactHirukawa()
+{
+ n_it_ = 5; // number of iteration max to converge
+ sampling_period_ = 0.005; // sampling period in seconds
+ jac_LF = Jac_LF::Jacobian::Zero(); // init the left foot jacobian
+ jac_RF = Jac_RF::Jacobian::Zero(); // init the right foot jacobian
+ jac_LH = Jac_LH::Jacobian::Zero(); // init the left arm jacobian
+ jac_RH = Jac_RH::Jacobian::Zero();; // init the right arm jacobian
+ //jacobian_ = Robot_Jacobian::Zero();
+}
+
+MultiContactHirukawa::~MultiContactHirukawa()
+{
+}
+
+int MultiContactHirukawa::InverseKinematicsOnLimbs(std::vector< FootAbsolutePosition > & rf,
+ std::vector< FootAbsolutePosition > & lf,
+ std::vector< HandAbsolutePosition > & rh,
+ std::vector< HandAbsolutePosition > & lh,
+ unsigned int currentIndex)
+{
+ cout << "q = \n" << q_ << endl ;
+ //metapod::jac< Robot_Model>::run(*robot_, jacobian_);
+ Jac_LF::run(*robot_, q_, metapod::Vector3dTpl<LocalFloatType>::Type(0,0,0), jac_LF);
+ Jac_RF::run(*robot_, q_, metapod::Vector3dTpl<LocalFloatType>::Type(0,0,0), jac_RF);
+ Jac_LH::run(*robot_, q_, metapod::Vector3dTpl<LocalFloatType>::Type(0,0,0), jac_LH);
+ Jac_RH::run(*robot_, q_, metapod::Vector3dTpl<LocalFloatType>::Type(0,0,0), jac_RH);
+
+ cout << "jac_LF = \n" << jac_LF << endl ;
+ cout << "jac_RF = \n" << jac_RF << endl ;
+ cout << "jac_LH = \n" << jac_LH << endl ;
+ cout << "jac_RH = \n" << jac_RH << endl ;
+ return 0 ;
+}
+
+int MultiContactHirukawa::DetermineContact(std::vector< FootAbsolutePosition > & rf,
+ std::vector< FootAbsolutePosition > & lf,
+ std::vector< HandAbsolutePosition > & rh,
+ std::vector< HandAbsolutePosition > & lh)
+{
+ contactVec_.resize(rf.size());
+ for (unsigned int i = 0 ; i < contactVec_.size() ; ++i )
+ {
+ contactVec_[i].clear();
+ if ( rf[i].z == 0.0 )
+ {
+ Contact aContact ;
+ aContact.n(0) = 0.0 ;
+ aContact.n(1) = 0.0 ;
+ aContact.n(2) = 1.0 ;
+
+ aContact.p(0) = rf[i].x ;
+ aContact.p(1) = rf[i].y ;
+ aContact.p(2) = rf[i].z ;
+ contactVec_[i].push_back(aContact) ;
+ }
+ if ( lf[i].z == 0.0 )
+ {
+ Contact aContact ;
+ aContact.n(0) = 0.0 ;
+ aContact.n(1) = 0.0 ;
+ aContact.n(2) = 1.0 ;
+
+ aContact.p(0) = lf[i].x ;
+ aContact.p(1) = lf[i].y ;
+ aContact.p(2) = lf[i].z ;
+ contactVec_[i].push_back(aContact) ;
+ }
+ if ( rh[i].stepType < 0.0 )
+ {
+ Contact aContact ;
+ aContact.n(0) = 0.0 ;
+ aContact.n(1) = 0.0 ;
+ aContact.n(2) = 1.0 ;
+
+ aContact.p(0) = rh[i].x ;
+ aContact.p(1) = rh[i].y ;
+ aContact.p(2) = rh[i].z ;
+ contactVec_[i].push_back(aContact) ;
+ }
+ if ( lh[i].stepType < 0.0 )
+ {
+ Contact aContact ;
+ aContact.n(0) = 0.0 ;
+ aContact.n(1) = 0.0 ;
+ aContact.n(2) = 1.0 ;
+
+ aContact.p(0) = lh[i].x ;
+ aContact.p(1) = lh[i].y ;
+ aContact.p(2) = lh[i].z ;
+ contactVec_[i].push_back(aContact) ;
+ }
+ }
+
+#ifdef VERBOSE
+ cout << "contactVec_.size() = " << contactVec_.size() << endl ;
+ for ( unsigned int i=0 ; i < contactVec_.size() ; ++i )
+ {
+ for ( unsigned int j=0 ; j < contactVec_[i].size() ; ++j )
+ {
+ cout << j << " : ["
+ << contactVec_[i][j].p(0) << " , "
+ << contactVec_[i][j].p(1) << " , "
+ << contactVec_[i][j].p(2) << "] ";
+ }
+ cout << endl ;
+ }
+#endif
+ return 0 ;
+}
+
+int MultiContactHirukawa::ForwardMomentum()
+{
+ return 0 ;
+}
+
+int MultiContactHirukawa::SolvingTheDynamics()
+{
+ return 0 ;
+}
+
+int MultiContactHirukawa::InverseMomentum()
+{
+ return 0 ;
+}
+
+int MultiContactHirukawa::online(vector<COMState> & comPos_,
+ vector<FootAbsolutePosition> & rf_,
+ vector<FootAbsolutePosition> & lf_,
+ vector<HandAbsolutePosition> & rh_,
+ vector<HandAbsolutePosition> & lh_)
+{
+ DetermineContact(rf_,lf_,rh_,lh_);
+// InverseKinematicsOnLimbs(rf_,lf_,rh_,lh_,0);
+
+ return 0 ;
+}
\ No newline at end of file
diff --git a/src/MultiContactRefTrajectoryGeneration/MultiContactHirukawa.hh b/src/MultiContactRefTrajectoryGeneration/MultiContactHirukawa.hh
new file mode 100644
index 0000000000000000000000000000000000000000..c17fd6e382a6c4135114cf90c2f211c065ada08f
--- /dev/null
+++ b/src/MultiContactRefTrajectoryGeneration/MultiContactHirukawa.hh
@@ -0,0 +1,107 @@
+#ifndef MULTICONTACTHIRUKAWA_HH
+#define MULTICONTACTHIRUKAWA_HH
+
+#include <sstream>
+#include <fstream>
+#include <jrl/walkgen/pgtypes.hh>
+#include <metapod/models/hrp2_14/hrp2_14.hh>
+#include <metapod/algos/jac_point_chain.hh>
+#include <metapod/tools/jcalc.hh>
+# include <metapod/algos/jac.hh>
+
+//#include "Clock.hh"
+
+#ifndef METAPOD_TYPEDEF
+#define METAPOD_TYPEDEF
+ typedef double LocalFloatType;
+ typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
+ typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
+
+ typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::l_wrist >::type LhandNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::r_wrist >::type RhandNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::LARM_LINK0 >::type LshoulderNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::RARM_LINK0 >::type RshoulderNode;
+
+ typedef metapod::Nodes< Robot_Model, Robot_Model::LLEG_LINK0 >::type LhipNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::RLEG_LINK0 >::type RhipNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::l_ankle >::type LankleNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::r_ankle >::type RankleNode;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::l_ankle, Robot_Model::BODY,0,true,false> Jac_LF;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_ankle, Robot_Model::RLEG_LINK0,0,true,false> Jac_RF;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::l_wrist, Robot_Model::LARM_LINK0,0,true,false> Jac_LH;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_wrist, Robot_Model::RARM_LINK0,0,true,false> Jac_RH;
+
+ typedef Eigen::Matrix<LocalFloatType, 6 * Robot_Model::NBBODIES, Robot_Model::NBDOF> Robot_Jacobian;
+#endif
+
+typedef Eigen::Matrix<LocalFloatType,6,1> vector6d ;
+
+namespace PatternGeneratorJRL {
+
+struct Contact_s
+{
+ Eigen::Matrix<LocalFloatType,3,1> p ; // position of the contact
+ Eigen::Matrix<LocalFloatType,3,1> n ; // normal vector of the contact surface
+};
+typedef struct Contact_s Contact;
+
+class MultiContactHirukawa
+{
+public:
+ MultiContactHirukawa();
+
+ ~MultiContactHirukawa();
+
+ int online(std::vector<COMState> & comPos_,
+ std::vector<FootAbsolutePosition> & rf_,
+ std::vector<FootAbsolutePosition> & lf_,
+ std::vector<HandAbsolutePosition> & rh_,
+ std::vector<HandAbsolutePosition> & lh_);
+ int InverseKinematicsOnLimbs(std::vector<FootAbsolutePosition> & rf,
+ std::vector<FootAbsolutePosition> & lf,
+ std::vector<HandAbsolutePosition> & rh,
+ std::vector<HandAbsolutePosition> & lh,
+ unsigned int currentIndex);
+ int DetermineContact(std::vector<FootAbsolutePosition> & rf,
+ std::vector<FootAbsolutePosition> & lf,
+ std::vector<HandAbsolutePosition> & rh,
+ std::vector<HandAbsolutePosition> & lh);
+ int ForwardMomentum();
+ int SolvingTheDynamics();
+ int InverseMomentum();
+
+private :
+ //robot model an configurations
+ Robot_Model * robot_ ;
+ Robot_Model::confVector q_, dq_ ;
+ Jac_LF::Jacobian jac_LF ;
+ Jac_RF::Jacobian jac_RF ;
+ Jac_LH::Jacobian jac_LH ;
+ Jac_RH::Jacobian jac_RH ;
+ //Robot_Jacobian jacobian_ ;
+
+ unsigned int n_it_ ; // number of iteration max to converge
+ double sampling_period_ ; // sampling period in seconds
+ std::vector< std::vector< Contact > > contactVec_ ;
+ std::vector< vector6d , Eigen::aligned_allocator<vector6d> > rf_vel_ ;
+ std::vector< vector6d , Eigen::aligned_allocator<vector6d> > lf_vel_ ;
+ std::vector< vector6d , Eigen::aligned_allocator<vector6d> > rh_vel_ ;
+ std::vector< vector6d , Eigen::aligned_allocator<vector6d> > lh_vel_ ;
+ std::vector< Eigen::MatrixXd > v_com ;
+
+public :
+ void q(Robot_Model::confVector & q)
+ {q_ = q;}
+};
+
+}
+#endif // HIRUKAWA2007_HH
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 907bc8afd8fac8f3dec4b24aa148a4d45c62b142..bd2c983c24f39ca7c8ce0acb694dc8c4af41c102 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -563,7 +563,6 @@ computing the analytical trajectories. */
/*! Set the current time reference for the analytical trajectory. */
double TimeShift = m_Tsingle*2;
- //double TimeShift = m_Tsingle;
m_AbsoluteTimeReference = m_CurrentTime-TimeShift;
m_AnalyticalZMPCoGTrajectoryX->SetAbsoluteTimeReference(m_AbsoluteTimeReference);
m_AnalyticalZMPCoGTrajectoryY->SetAbsoluteTimeReference(m_AbsoluteTimeReference);
@@ -573,10 +572,12 @@ computing the analytical trajectories. */
FillQueues(m_CurrentTime,m_CurrentTime+m_PreviewControlTime-TimeShift,
ZMPPositions, COMStates,LeftFootAbsolutePositions, RightFootAbsolutePositions);
+ //deque<COMState> filteredCoM = COMStates ;
+
+ /*! initialize the dynamic filter */
unsigned int n = COMStates.size();
double KajitaPCpreviewWindow = 1.6 ;
- m_kajitaDynamicFilter->init( m_CurrentTime,
- m_SamplingPeriod,
+ m_kajitaDynamicFilter->init( m_SamplingPeriod,
m_SamplingPeriod,
(double)(n+1)*m_SamplingPeriod,
KajitaPCpreviewWindow,
@@ -584,12 +585,13 @@ computing the analytical trajectories. */
InitLeftFootAbsolutePosition,
lStartingCOMState );
+ /*! Set the upper body trajectory */
CjrlHumanoidDynamicRobot * aHDR = m_kajitaDynamicFilter->
getComAndFootRealization()->getHumanoidDynamicRobot();
MAL_VECTOR_TYPE(double) UpperConfig = aHDR->currentConfiguration() ;
MAL_VECTOR_TYPE(double) UpperVel = aHDR->currentVelocity() ;
MAL_VECTOR_TYPE(double) UpperAcc = aHDR->currentAcceleration() ;
-// // carry the weight in front of him
+ // carry the weight in front of him
UpperConfig(18)= 0.0 ; // CHEST_JOINT0
UpperConfig(19)= 0.015 ; // CHEST_JOINT1
UpperConfig(20)= 0.0 ; // HEAD_JOINT0
@@ -634,8 +636,6 @@ computing the analytical trajectories. */
UpperAcc(i)=0.0;
}
-
-
m_kajitaDynamicFilter->setRobotUpperPart(UpperConfig,UpperVel,UpperAcc);
/*! Add "KajitaPCpreviewWindow" second to the buffers for fitering */
@@ -650,37 +650,26 @@ computing the analytical trajectories. */
LeftFootAbsolutePositions.push_back(lastLF);
RightFootAbsolutePositions.push_back(lastRF);
}
- unsigned int N = ZMPPositions.size();
- int stage0 = 0 ;
- vector <vector<double> > ZMPMB (N , vector<double> (2,0.0)) ;
- for (unsigned int i = 0 ; i < N ; ++i)
- {
- m_kajitaDynamicFilter->ComputeZMPMB( m_SamplingPeriod, COMStates[i],
- LeftFootAbsolutePositions[i], RightFootAbsolutePositions[i],
- ZMPMB[i] , stage0 , i);
- }
- m_kajitaDynamicFilter->getClock()->Display();
- deque<ZMPPosition> inputdeltaZMP_deq(N) ;
deque<COMState> outputDeltaCOMTraj_deq ;
- for (unsigned int i = 0 ; i < N ; ++i)
- {
- inputdeltaZMP_deq[i].px = ZMPPositions[i].px - ZMPMB[i][0] ;
- inputdeltaZMP_deq[i].py = ZMPPositions[i].py - ZMPMB[i][1] ;
- inputdeltaZMP_deq[i].pz = 0.0 ;
- inputdeltaZMP_deq[i].theta = 0.0 ;
- inputdeltaZMP_deq[i].time = m_CurrentTime + i * m_SamplingPeriod ;
- inputdeltaZMP_deq[i].stepType = ZMPPositions[i].stepType ;
- }
- m_kajitaDynamicFilter->OptimalControl(inputdeltaZMP_deq,outputDeltaCOMTraj_deq) ;
+ m_kajitaDynamicFilter->OffLinefilter(
+ m_CurrentTime,
+ COMStates,
+ ZMPPositions,
+ LeftFootAbsolutePositions,
+ RightFootAbsolutePositions,
+ vector< MAL_VECTOR_TYPE(double) > (1,UpperConfig),
+ vector< MAL_VECTOR_TYPE(double) > (1,UpperConfig),
+ vector< MAL_VECTOR_TYPE(double) > (1,UpperConfig),
+ outputDeltaCOMTraj_deq);
vector <vector<double> > filteredZMPMB (n , vector<double> (2,0.0)) ;
for (unsigned int i = 0 ; i < n ; ++i)
{
for(int j=0;j<3;j++)
{
-// COMStates[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
-// COMStates[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
+ COMStates[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
+ COMStates[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
}
}
@@ -689,15 +678,6 @@ computing the analytical trajectories. */
{
m_UpperTimeLimitToUpdateStacks += m_DeltaTj[i];
}
-
- for (unsigned int i = 0 ; i < KajitaPCpreviewWindow/m_SamplingPeriod ; ++i)
- {
- ZMPPositions.pop_back();
- COMStates.pop_back();
- LeftFootAbsolutePositions.pop_back();
- RightFootAbsolutePositions.pop_back();
- }
-
}
int AnalyticalMorisawaCompact::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
@@ -763,7 +743,7 @@ When the limit is reached, and the stack exhausted this method is called again.
/*! Recompute time when a new step should be added. */
m_UpperTimeLimitToUpdateStacks = m_AbsoluteTimeReference + m_DeltaTj[0] + m_Tdble + 0.45 * m_Tsingle;
- m_kajitaDynamicFilter->init( m_CurrentTime, m_SamplingPeriod, 0.04, m_SamplingPeriod, 1.6,
+ m_kajitaDynamicFilter->init( m_SamplingPeriod, 0.04, m_SamplingPeriod, 1.6,
lStartingCOMState.z[0], InitLeftFootAbsolutePosition, lStartingCOMState );
return m_RelativeFootPositions.size();
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index b04444a01ced8483db941e2cc2a9cfe3b7f2b6d3..2be47df54bdea181a7b6717bff53106684e86a08 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -9,7 +9,6 @@ DynamicFilter::DynamicFilter(
SimplePluginManager *SPM,
CjrlHumanoidDynamicRobot *aHS)
{
- currentTime_ = 0.0 ;
controlPeriod_ = 0.0 ;
interpolationPeriod_ = 0.0 ;
previewWindowSize_ = 0.0 ;
@@ -24,6 +23,8 @@ DynamicFilter::DynamicFilter(
comAndFootRealization_->SetHeightOfTheCoM(CoMHeight_);
comAndFootRealization_->setSamplingPeriod(interpolationPeriod_);
comAndFootRealization_->Initialization();
+ stage0_ = 0 ;
+ stage1_ = 1 ;
PC_ = new PreviewControl(
SPM,OptimalControllerSolver::MODE_WITH_INITIALPOS,false);
@@ -49,8 +50,8 @@ DynamicFilter::DynamicFilter(
DInitX_ = 0.0 ;
DInitY_ = 0.0 ;
- jacobian_lf_ = Jacobian_LF::Jacobian::Zero();
- jacobian_rf_ = Jacobian_RF::Jacobian::Zero();
+ jacobian_lf_ = Jac_LF::Jacobian::Zero();
+ jacobian_rf_ = Jac_RF::Jacobian::Zero();
sxzmp_ = 0.0 ;
syzmp_ = 0.0 ;
@@ -74,23 +75,9 @@ DynamicFilter::~DynamicFilter()
}
}
-void DynamicFilter::InverseDynamics(MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration)
-{
- Robot_Model::confVector q, dq, ddq ;
- for(unsigned int j = 0 ; j < configuration.size() ; j++ )
- {
- q(j,0) = configuration(j) ;
- dq(j,0) = velocity(j) ;
- ddq(j,0) = acceleration(j) ;
- }
- metapod::rnea< Robot_Model, true >::run(robot_, q, dq, ddq);
-}
-
-void DynamicFilter::setRobotUpperPart(MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration)
+void DynamicFilter::setRobotUpperPart(const MAL_VECTOR_TYPE(double) & configuration,
+ const MAL_VECTOR_TYPE(double) & velocity,
+ const MAL_VECTOR_TYPE(double) & acceleration)
{
for ( unsigned int i = 0 ; i < upperPartIndex.size() ; ++i )
{
@@ -103,7 +90,6 @@ void DynamicFilter::setRobotUpperPart(MAL_VECTOR_TYPE(double) & configuration,
/// \brief Initialise all objects, to be called just after the constructor
void DynamicFilter::init(
- double currentTime,
double controlPeriod,
double interpolationPeriod,
double PG_T,
@@ -112,7 +98,6 @@ void DynamicFilter::init(
FootAbsolutePosition inputLeftFoot,
COMState inputCoMState)
{
- currentTime_ = currentTime ;
controlPeriod_ = controlPeriod ;
interpolationPeriod_ = interpolationPeriod ;
PG_T_ = PG_T ;
@@ -161,7 +146,6 @@ void DynamicFilter::init(
prev_dq_ = dq_ ;
prev_ddq_ = ddq_ ;
jcalc<Robot_Model>::run(robot_,q_ ,dq_ );
- jcalc<Robot_Model>::run(robot_2,q_, dq_);
deltaZMP_deq_.resize( PG_N_);
ZMPMB_vec_.resize( PG_N_, vector<double>(2));
@@ -201,32 +185,53 @@ void DynamicFilter::init(
}
int DynamicFilter::OffLinefilter(
- COMState & lastCtrlCoMState,
- FootAbsolutePosition & lastCtrlLeftFoot,
- FootAbsolutePosition & lastCtrlRightFoot,
- deque<COMState> & inputCOMTraj_deq_,
- deque<ZMPPosition> inputZMPTraj_deq_,
- deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ const double currentTime,
+ const deque<COMState> &inputCOMTraj_deq_,
+ const deque<ZMPPosition> &inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> &inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> &inputRightFootTraj_deq_,
+ const vector< MAL_VECTOR_TYPE(double) > & UpperPart_q,
+ const vector< MAL_VECTOR_TYPE(double) > & UpperPart_dq,
+ const vector< MAL_VECTOR_TYPE(double) > & UpperPart_ddq,
deque<COMState> & outputDeltaCOMTraj_deq_)
{
- // TODO implement the filter in one single function for offline walking
+ unsigned int N = inputCOMTraj_deq_.size() ;
+ ZMPMB_vec_.resize(N) ;
+ deltaZMP_deq_.resize(N);
+
+ setRobotUpperPart(UpperPart_q[0],UpperPart_dq[0],UpperPart_ddq[0]);
+
+ for(unsigned int i = 0 ; i < N ; ++i )
+ {
+ ComputeZMPMB(interpolationPeriod_,inputCOMTraj_deq_[i],inputLeftFootTraj_deq_[i],
+ inputRightFootTraj_deq_[i], ZMPMB_vec_[i] , stage0_ , i);
+ }
+ for (unsigned int i = 0 ; i < N ; ++i)
+ {
+ deltaZMP_deq_[i].px = inputZMPTraj_deq_[i].px - ZMPMB_vec_[i][0] ;
+ deltaZMP_deq_[i].py = inputZMPTraj_deq_[i].py - ZMPMB_vec_[i][1] ;
+ deltaZMP_deq_[i].pz = 0.0 ;
+ deltaZMP_deq_[i].theta = 0.0 ;
+ deltaZMP_deq_[i].time = currentTime + i * interpolationPeriod_ ;
+ deltaZMP_deq_[i].stepType = inputZMPTraj_deq_[i].stepType ;
+ }
+ OptimalControl(deltaZMP_deq_,outputDeltaCOMTraj_deq_) ;
+
return 0;
}
-int DynamicFilter::OnLinefilter(
- const deque<COMState> & ctrlCoMState,
- const deque<FootAbsolutePosition> & ctrlLeftFoot,
- const deque<FootAbsolutePosition> & ctrlRightFoot,
- const deque<COMState> & inputCOMTraj_deq_,
- const deque<ZMPPosition> inputZMPTraj_deq_,
- const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- deque<COMState> & outputDeltaCOMTraj_deq_)
-{
- // TODO implement the filter in one single function for online walking
- return 0 ;
-}
+//int DynamicFilter::OnLinefilter(
+// const deque<COMState> & ctrlCoMState,
+// const deque<FootAbsolutePosition> & ctrlLeftFoot,
+// const deque<FootAbsolutePosition> & ctrlRightFoot,
+// const deque<COMState> & inputCOMTraj_deq_,
+// const deque<ZMPPosition> inputZMPTraj_deq_,
+// const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+// const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+// deque<COMState> & outputDeltaCOMTraj_deq_)
+//{
+// return 0 ;
+//}
void DynamicFilter::InverseKinematics(
const COMState & inputCoMState,
@@ -239,6 +244,8 @@ void DynamicFilter::InverseKinematics(
int stage,
int iteration)
{
+
+ // lower body
aCoMState_(0) = inputCoMState.x[0]; aCoMSpeed_(0) = inputCoMState.x[1];
aCoMState_(1) = inputCoMState.y[0]; aCoMSpeed_(1) = inputCoMState.y[1];
aCoMState_(2) = inputCoMState.z[0]; aCoMSpeed_(2) = inputCoMState.z[1];
@@ -266,28 +273,18 @@ void DynamicFilter::InverseKinematics(
configuration, velocity, acceleration,
iteration, stage);
+ // upper body
if (walkingHeuristic_)
{
LankleNode & node_lankle = boost::fusion::at_c<Robot_Model::l_ankle>(robot_.nodes);
RankleNode & node_rankle = boost::fusion::at_c<Robot_Model::r_ankle>(robot_.nodes);
- LhandNode & node_lhand = boost::fusion::at_c<Robot_Model::l_wrist>(robot_.nodes);
- RhandNode & node_rhand = boost::fusion::at_c<Robot_Model::r_wrist>(robot_.nodes);
-
- LshoulderNode & node_lshoulder = boost::fusion::at_c<Robot_Model::LARM_LINK0>(robot_.nodes);
- RshoulderNode & node_rshoulder = boost::fusion::at_c<Robot_Model::RARM_LINK0>(robot_.nodes);
-
RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
Spatial::TransformT<LocalFloatType,Spatial::RotationMatrixTpl<LocalFloatType> > waistXlf ;
Spatial::TransformT<LocalFloatType,Spatial::RotationMatrixTpl<LocalFloatType> > waistXrf ;
waistXlf = node_waist.body.iX0 * node_lankle.body.iX0.inverse() ;
waistXrf = node_waist.body.iX0 * node_rankle.body.iX0.inverse() ;
-//
-// Spatial::TransformT<LocalFloatType,Spatial::RotationMatrixTpl<LocalFloatType> > shoulderXlh ;
-// Spatial::TransformT<LocalFloatType,Spatial::RotationMatrixTpl<LocalFloatType> > shoulderXrh ;
-// shoulderXlh = node_lshoulder.body.iX0 * node_lhand.body.iX0.inverse() ;
-// shoulderXrh = node_rshoulder.body.iX0 * node_rhand.body.iX0.inverse() ;
// Homogeneous matrix
matrix4d identity,leftHandPose, rightHandPose;
@@ -299,8 +296,6 @@ void DynamicFilter::InverseKinematics(
MAL_S4x4_MATRIX_ACCESS_I_J(leftHandPose,1,3) = 0.0;
MAL_S4x4_MATRIX_ACCESS_I_J(leftHandPose,2,3) = -0.45;
-
-
MAL_S4x4_MATRIX_ACCESS_I_J(rightHandPose,0,3) = -4*waistXlf.r()(0);
MAL_S4x4_MATRIX_ACCESS_I_J(rightHandPose,1,3) = 0.0;
MAL_S4x4_MATRIX_ACCESS_I_J(rightHandPose,2,3) = -0.45;
@@ -309,7 +304,6 @@ void DynamicFilter::InverseKinematics(
MAL_VECTOR_DIM(rarm_q, double, 6) ;
CjrlHumanoidDynamicRobot * HDR = comAndFootRealization_->getHumanoidDynamicRobot();
- int err1,err2;
CjrlJoint* left_shoulder = NULL ;
CjrlJoint* right_shoulder = NULL ;
@@ -323,9 +317,8 @@ void DynamicFilter::InverseKinematics(
right_shoulder = actuatedJoints[i];
}
-
- err1 = HDR->getSpecializedInverseKinematics( *left_shoulder ,*(HDR->leftWrist()), identity, leftHandPose, larm_q );
- err2 = HDR->getSpecializedInverseKinematics( *right_shoulder ,*(HDR->rightWrist()), identity, rightHandPose, rarm_q );
+ HDR->getSpecializedInverseKinematics( *left_shoulder ,*(HDR->leftWrist()), identity, leftHandPose, larm_q );
+ HDR->getSpecializedInverseKinematics( *right_shoulder ,*(HDR->rightWrist()), identity, rightHandPose, rarm_q );
// swinging arms
upperPartConfiguration_(upperPartIndex[0])= 0.0 ; // CHEST_JOINT0
@@ -381,8 +374,8 @@ void DynamicFilter::ComputeZMPMB(
const FootAbsolutePosition & inputLeftFoot,
const FootAbsolutePosition & inputRightFoot,
vector<double> & ZMPMB,
- int stage,
- int iteration)
+ unsigned int stage,
+ unsigned int iteration)
{
InverseKinematics( inputCoMState, inputLeftFoot, inputRightFoot,
ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_,
@@ -397,19 +390,21 @@ void DynamicFilter::ComputeZMPMB(
}
//computeWaist( inputLeftFoot );
- RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
+
// Apply the RNEA on the robot model
- clock_.StartTiming();
+ RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
metapod::rnea< Robot_Model, true >::run(robot_, q_, dq_, ddq_);
- clock_.StopTiming();
-
- clock_.IncIteration();
+ // extract the CoM momentum and forces
m_force = node_waist.body.iX0.applyInv(node_waist.joint.f);
+ metapod::Vector3dTpl< LocalFloatType >::Type CoM_Waist_vec (node_waist.body.iX0.r() - com ()) ;
+ metapod::Vector3dTpl< LocalFloatType >::Type CoM_torques (0.0,0.0,0.0);
+ CoM_torques = m_force.n() + metapod::Skew<LocalFloatType>(CoM_Waist_vec) * m_force.f() ;
+ // compute the Multibody ZMP
ZMPMB.resize(2);
- ZMPMB[0] = - m_force.n()[1] / m_force.f()[2] ;
- ZMPMB[1] = m_force.n()[0] / m_force.f()[2] ;
+ ZMPMB[0] = - CoM_torques[1] / m_force.f()[2] ;
+ ZMPMB[1] = CoM_torques[0] / m_force.f()[2] ;
return ;
}
@@ -452,3 +447,175 @@ int DynamicFilter::OptimalControl(
}
return 0 ;
}
+
+// TODO finish the implementation of a better waist tracking
+void DynamicFilter::computeWaist(const FootAbsolutePosition & inputLeftFoot)
+{
+ Eigen::Matrix< LocalFloatType, 6, 1 > waist_speed, waist_acc ;
+ Eigen::Matrix< LocalFloatType, 3, 1 > waist_theta ;
+ // compute the speed and acceleration of the waist in the world frame
+ if (PreviousSupportFoot_)
+ {
+ Jac_LF::run(robot_, prev_q_, Vector3dTpl<LocalFloatType>::Type(0,0,0), jacobian_lf_);
+ waist_speed = jacobian_lf_ * prev_dq_ ;
+ waist_acc = jacobian_lf_ * prev_ddq_ /* + d_jacobian_lf_ * prev_dq_*/ ;
+ }else
+ {
+ Jac_RF::run(robot_, prev_q_, Vector3dTpl<LocalFloatType>::Type(0,0,0), jacobian_rf_);
+ waist_speed = jacobian_rf_ * prev_dq_ ;
+ waist_acc = jacobian_rf_ * prev_ddq_ /*+ d_jacobian_rf_ * prev_dq_*/ ;
+ }
+ for (unsigned int i = 0 ; i < 6 ; ++i)
+ {
+ dq_(i,0) = waist_speed(i,0);
+ ddq_(i,0) = waist_acc(i,0);
+ }
+ // compute the position of the waist in the world frame
+ RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
+ waist_theta(0,0) = prev_q_(3,0) ;
+ waist_theta(1,0) = prev_dq_(4,0) ;
+ waist_theta(2,0) = prev_ddq_(5,0) ;
+ q_(0,0) = node_waist.body.iX0.inverse().r()(0,0) ;
+ q_(1,0) = node_waist.body.iX0.inverse().r()(1,0) ;
+ q_(2,0) = node_waist.body.iX0.inverse().r()(2,0) ;
+ q_(3,0) = waist_theta(0,0) ;
+ q_(4,0) = waist_theta(1,0) ;
+ q_(5,0) = waist_theta(2,0) ;
+
+ if (inputLeftFoot.stepType<0)
+ {
+ PreviousSupportFoot_ = true ; // left foot is supporting
+ }
+ else
+ {
+ PreviousSupportFoot_ = false ; // right foot is supporting
+ }
+ prev_q_ = q_ ;
+ prev_dq_ = dq_ ;
+ prev_ddq_ = ddq_ ;
+
+ // Robot_Model::confVector q, dq, ddq;
+ // for(unsigned int j = 0 ; j < 6 ; j++ )
+ // {
+ // q(j,0) = 0 ;
+ // dq(j,0) = 0 ;
+ // ddq(j,0) = 0 ;
+ // }
+ // for(unsigned int j = 6 ; j < ZMPMBConfiguration_.size() ; j++ )
+ // {
+ // q(j,0) = ZMPMBConfiguration_(j) ;
+ // dq(j,0) = ZMPMBVelocity_(j) ;
+ // ddq(j,0) = ZMPMBAcceleration_(j) ;
+ // }
+ //
+ // metapod::rnea< Robot_Model, true >::run(robot_2, q, dq, ddq);
+ // LankleNode & node_lankle = boost::fusion::at_c<Robot_Model::l_ankle>(robot_2.nodes);
+ // RankleNode & node_rankle = boost::fusion::at_c<Robot_Model::r_ankle>(robot_2.nodes);
+ //
+ // CWx = node_waist.body.iX0.r()(0,0) - inputCoMState.x[0] ;
+ // CWy = node_waist.body.iX0.r()(1,0) - inputCoMState.y[0] ;
+ //
+ // // Debug Purpose
+ // // -------------
+ // ofstream aof;
+ // string aFileName;
+ // ostringstream oss(std::ostringstream::ate);
+ // static int it = 0;
+ //
+ // // --------------------
+ // oss.str("DynamicFilterMetapodAccWaistSupportFoot.dat");
+ // aFileName = oss.str();
+ // if(it == 0)
+ // {
+ // aof.open(aFileName.c_str(),ofstream::out);
+ // aof.close();
+ // }
+ // ///----
+ // aof.open(aFileName.c_str(),ofstream::app);
+ // aof.precision(8);
+ // aof.setf(ios::scientific, ios::floatfield);
+ // aof << filterprecision( it*samplingPeriod) << " " ; // 1
+ //
+ // if (inputLeftFoot.stepType < 0)
+ // {
+ // aof << filterprecision( node_lankle.body.ai.v()(0,0) ) << " " // 2
+ // << filterprecision( node_lankle.body.ai.v()(1,0) ) << " " // 3
+ // << filterprecision( node_lankle.body.ai.v()(2,0) ) << " " // 4
+ // << filterprecision( node_lankle.body.ai.w()(0,0) ) << " " // 5
+ // << filterprecision( node_lankle.body.ai.w()(1,0) ) << " " // 6
+ // << filterprecision( node_lankle.body.ai.w()(2,0) ) << " "; // 7
+ // }else
+ // {
+ // aof << filterprecision( node_rankle.body.ai.v()(0,0) ) << " " // 2
+ // << filterprecision( node_rankle.body.ai.v()(1,0) ) << " " // 3
+ // << filterprecision( node_rankle.body.ai.v()(2,0) ) << " " // 4
+ // << filterprecision( node_rankle.body.ai.w()(0,0) ) << " " // 5
+ // << filterprecision( node_rankle.body.ai.w()(1,0) ) << " " // 6
+ // << filterprecision( node_rankle.body.ai.w()(2,0) ) << " " ;// 7
+ // }
+ //
+ // aof << filterprecision( inputCoMState.x[2] ) << " " // 8
+ // << filterprecision( inputCoMState.y[2] ) << " " // 9
+ // << filterprecision( inputCoMState.z[2] ) << " " // 10
+ // << filterprecision( inputCoMState.roll[2] ) << " " // 11
+ // << filterprecision( inputCoMState.pitch[2] ) << " " // 12
+ // << filterprecision( inputCoMState.yaw[2] ) << " " ; // 13
+ //
+ // if (inputLeftFoot.stepType < 0)
+ // {
+ // aof << filterprecision( node_lankle.body.vi.v()(0,0) ) << " " // 14
+ // << filterprecision( node_lankle.body.vi.v()(1,0) ) << " " // 15
+ // << filterprecision( node_lankle.body.vi.v()(2,0) ) << " " // 16
+ // << filterprecision( node_lankle.body.vi.w()(0,0) ) << " " // 17
+ // << filterprecision( node_lankle.body.vi.w()(1,0) ) << " " // 18
+ // << filterprecision( node_lankle.body.vi.w()(2,0) ) << " " ;// 19
+ // }else
+ // {
+ // aof << filterprecision( node_rankle.body.vi.v()(0,0) ) << " " // 14
+ // << filterprecision( node_rankle.body.vi.v()(1,0) ) << " " // 15
+ // << filterprecision( node_rankle.body.vi.v()(2,0) ) << " " // 16
+ // << filterprecision( node_rankle.body.vi.w()(0,0) ) << " " // 17
+ // << filterprecision( node_rankle.body.vi.w()(1,0) ) << " " // 18
+ // << filterprecision( node_rankle.body.vi.w()(2,0) ) << " "; // 19
+ // }
+ //
+ // aof << filterprecision( inputCoMState.x[1] ) << " " // 20
+ // << filterprecision( inputCoMState.y[1] ) << " " // 21
+ // << filterprecision( inputCoMState.z[1] ) << " " // 22
+ // << filterprecision( inputCoMState.roll[1] ) << " " // 23
+ // << filterprecision( inputCoMState.pitch[1] ) << " " // 24
+ // << filterprecision( inputCoMState.yaw[1] ) << " " ; // 25
+ //
+ // aof << filterprecision( node_waist.joint.vj.v()(0,0) ) << " " // 26
+ // << filterprecision( node_waist.joint.vj.v()(1,0) ) << " " // 27
+ // << filterprecision( node_waist.joint.vj.v()(2,0) ) << " " // 28
+ // << filterprecision( node_waist.joint.vj.w()(0,0) ) << " " // 29
+ // << filterprecision( node_waist.joint.vj.w()(1,0) ) << " " // 30
+ // << filterprecision( node_waist.joint.vj.w()(2,0) ) << " "; // 31
+ //
+ // aof << filterprecision( inputCoMState.x[0] ) << " " // 32
+ // << filterprecision( inputCoMState.y[0] ) << " " // 33
+ // << filterprecision( inputCoMState.z[0] ) << " " // 34
+ // << filterprecision( inputCoMState.roll[0] ) << " " // 35
+ // << filterprecision( inputCoMState.pitch[0] ) << " " // 36
+ // << filterprecision( inputCoMState.yaw[0] ) << " " ; // 37
+ //
+ // aof << filterprecision( node_waist.body.iX0.r()(0,0) ) << " " // 38
+ // << filterprecision( node_waist.body.iX0.r()(1,0) ) << " " // 39
+ // << filterprecision( node_waist.body.iX0.r()(2,0) ) << " ";// 40
+ //
+ //
+ // for(unsigned int j = 0 ; j < ZMPMBConfiguration_.size() ; j++ )//41
+ // aof << filterprecision( ZMPMBConfiguration_(j) ) << " " ;
+ // for(unsigned int j = 0 ; j < ZMPMBVelocity_.size() ; j++ ) //77
+ // aof << filterprecision( ZMPMBVelocity_(j) ) << " " ;
+ // for(unsigned int j = 0 ; j < ZMPMBAcceleration_.size() ; j++ )//113
+ // aof << filterprecision( ZMPMBAcceleration_(j) ) << " " ;
+ //
+ //
+ // aof << endl ;
+ // aof.close();
+ // ++it;
+
+ return ;
+}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index 20fe6bfa50891dc776428534ffc7423e824e339f..94f672c77637ee8f97fc4a2115ab592092ba86b4 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -6,24 +6,34 @@
#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
#include <metapod/algos/jac_point_chain.hh>
#include "Clock.hh"
+#include <boost/fusion/include/accumulate.hpp>
#ifndef METAPOD_TYPEDEF
#define METAPOD_TYPEDEF
- typedef double LocalFloatType;
- typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
- typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
- typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
- typedef metapod::Nodes< Robot_Model, Robot_Model::l_ankle >::type LankleNode;
- typedef metapod::Nodes< Robot_Model, Robot_Model::r_ankle >::type RankleNode;
- typedef metapod::Nodes< Robot_Model, Robot_Model::l_wrist >::type LhandNode;
- typedef metapod::Nodes< Robot_Model, Robot_Model::r_wrist >::type RhandNode;
- typedef metapod::Nodes< Robot_Model, Robot_Model::LARM_LINK0 >::type LshoulderNode;
- typedef metapod::Nodes< Robot_Model, Robot_Model::RARM_LINK0 >::type RshoulderNode;
-
- typedef metapod::jac_point_chain < Robot_Model,
- Robot_Model::l_ankle, Robot_Model::BODY,0,true,false> Jacobian_LF;
- typedef metapod::jac_point_chain < Robot_Model,
- Robot_Model::r_ankle, Robot_Model::BODY,0,true,false> Jacobian_RF;
+ typedef double LocalFloatType;
+ typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
+ typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
+
+ typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::l_wrist >::type LhandNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::r_wrist >::type RhandNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::LARM_LINK0 >::type LshoulderNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::RARM_LINK0 >::type RshoulderNode;
+
+ typedef metapod::Nodes< Robot_Model, Robot_Model::LLEG_LINK0 >::type LhipNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::RLEG_LINK0 >::type RhipNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::l_ankle >::type LankleNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::r_ankle >::type RankleNode;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::l_ankle, Robot_Model::LLEG_LINK0,0,true,false> Jac_LF;
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_ankle, Robot_Model::RLEG_LINK0,0,true,false> Jac_RF;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::l_wrist, Robot_Model::LARM_LINK0,0,true,false> Jac_LH;
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_wrist, Robot_Model::RARM_LINK0,0,true,false> Jac_RH;
#endif
@@ -44,13 +54,14 @@ namespace PatternGeneratorJRL
~DynamicFilter();
/// \brief
int OffLinefilter(
- COMState & lastCtrlCoMState,
- FootAbsolutePosition & lastCtrlLeftFoot,
- FootAbsolutePosition & lastCtrlRightFoot,
- deque<COMState> & inputCOMTraj_deq_,
- deque<ZMPPosition> inputZMPTraj_deq_,
- deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ const double currentTime,
+ const deque<COMState> & inputCOMTraj_deq_,
+ const deque<ZMPPosition> & inputZMPTraj_deq_,
+ const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ const vector<MAL_VECTOR_TYPE(double) > &UpperPart_q,
+ const vector<MAL_VECTOR_TYPE(double) > &UpperPart_dq,
+ const vector<MAL_VECTOR_TYPE(double) > &UpperPart_ddq,
deque<COMState> & outputDeltaCOMTraj_deq_);
int OnLinefilter(
@@ -64,7 +75,6 @@ namespace PatternGeneratorJRL
deque<COMState> & outputDeltaCOMTraj_deq_);
void init(
- double currentTime,
double controlPeriod,
double interpolationPeriod,
double PG_T,
@@ -93,8 +103,8 @@ namespace PatternGeneratorJRL
const FootAbsolutePosition & inputLeftFoot,
const FootAbsolutePosition & inputRightFoot,
vector<double> & ZMPMB,
- int stage,
- int iteration);
+ unsigned int stage,
+ unsigned int iteration);
void stage0INstage1();
@@ -103,10 +113,6 @@ namespace PatternGeneratorJRL
deque<ZMPPosition> & inputdeltaZMP_deq,
deque<COMState> & outputDeltaCOMTraj_deq_);
- void InverseDynamics(MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration);
-
private: // Private methods
/// \brief calculate, from the CoM computed by the preview control,
@@ -123,14 +129,12 @@ namespace PatternGeneratorJRL
/// given by the function "filter"
void InverseDynamics(deque<ZMPPosition> inputZMPTraj_deq);
+ metapod::Vector3dTpl< LocalFloatType >::Type computeCoM();
// -------------------------------------------------------------------
public: // The accessors
/// \brief setter :
- inline void setCurrentTime(double time)
- {currentTime_ = time ;}
-
inline void setControlPeriod(double controlPeriod)
{controlPeriod_ = controlPeriod ;}
@@ -143,16 +147,13 @@ namespace PatternGeneratorJRL
inline void setPreviewWindowSize_(double previewWindowSize)
{ previewWindowSize_ = previewWindowSize; }
- void setRobotUpperPart(MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration);
+ void setRobotUpperPart(const MAL_VECTOR_TYPE(double) & configuration,
+ const MAL_VECTOR_TYPE(double) & velocity,
+ const MAL_VECTOR_TYPE(double) & acceleration);
/// \brief getter :
inline ComAndFootRealizationByGeometry * getComAndFootRealization()
- { return comAndFootRealization_;};
-
- inline double getCurrentTime()
- {return currentTime_ ;}
+ { return comAndFootRealization_;}
inline double getControlPeriod()
{return controlPeriod_ ;}
@@ -167,15 +168,23 @@ namespace PatternGeneratorJRL
{ errx = sxzmp_ ; erry = syzmp_ ; }
inline Clock * getClock()
- { return &clock_ ; };
+ { return &clock_ ; }
+
+
+ inline metapod::Vector3dTpl< LocalFloatType >::Type com ()
+ {
+ double sum_mass = 0.0 ;
+ metapod::Vector3dTpl< LocalFloatType >::Type com (0.0,0.0,0.0);
+ sum_mass = boost::fusion::accumulate(robot_.nodes , sum_mass , MassSum() );
+ com = boost::fusion::accumulate(robot_.nodes , com , MassbyComSum() );
+ return com / sum_mass ;
+ }
private: // Private members
/// \brief Time variables
/// -----------------------------------
- /// \brief Current time of the PG.
- double currentTime_ ;
-
+ ///
/// \brief control period of the PG host
double controlPeriod_;
@@ -234,12 +243,12 @@ namespace PatternGeneratorJRL
/// ---------------------------------
/// \brief Initialize the robot with the autogenerated files
/// by MetapodFromUrdf
- Robot_Model robot_,robot_2;
+ Robot_Model robot_;
/// \brief Initialize the robot leg jacobians with the
/// autogenerated files by MetapodFromUrdf
- Jacobian_LF::Jacobian jacobian_rf_ ;
- Jacobian_RF::Jacobian jacobian_lf_ ;
+ Jac_LF::Jacobian jacobian_rf_ ;
+ Jac_RF::Jacobian jacobian_lf_ ;
/// \brief force acting on the CoM of the robot expressed
/// in the Euclidean Frame
@@ -273,6 +282,34 @@ namespace PatternGeneratorJRL
/// \brief time measurement
Clock clock_;
+
+ /// \brief Stages, used in the analytical inverse kinematic.
+ unsigned int stage0_ ;
+ unsigned int stage1_ ;
+
+ private : // private struct
+ struct MassSum
+ {
+ typedef LocalFloatType result_type;
+
+ template <typename T>
+ result_type operator()( const result_type sum_mass, T & node) const
+ {
+ return ( sum_mass + Robot_Model::inertias[node.id].m() ) ;
+ }
+ };
+
+ struct MassbyComSum
+ {
+ typedef metapod::Vector3dTpl< LocalFloatType >::Type result_type;
+
+ template <typename T>
+ result_type operator()( const result_type sum_h, const T & x) const
+ {
+ double mass = Robot_Model::inertias[x.id].m() ;
+ return ( sum_h + mass * x.body.iX0.r() + x.body.iX0.E() * Robot_Model::inertias[x.id].h() );
+ }
+ };
};
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 63ca683f31903ffe1051c6e8f2cc5106ab6c0002..c23a02eebf1bab093e066dae5db42c5ab533091a 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -435,7 +435,7 @@ int
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
FinalCurrentStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
- dynamicFilter_->init(0.0,m_SamplingPeriod,InterpolationPeriod_,
+ dynamicFilter_->init(m_SamplingPeriod,InterpolationPeriod_,
QP_T_, QP_N_*QP_T_ - QP_T_/m_SamplingPeriod * InterpolationPeriod_ ,CoMHeight_,InitLeftFootAbsolutePosition,lStartingCOMState);
return 0;
}
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index a5b9f533d1d56ec09c7befbabfbb56d616c76a7a..4170faaa19acd3d73a320d28240e96c917d81402 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -169,7 +169,79 @@ ENDMACRO(ADD_HERDT_2010)
#ADD_HERDT_2010(TestHerdt2010)
ADD_HERDT_2010(TestHerdt2010DynamicFilter)
-ADD_HERDT_2010(TestInverseKinematics)
+
+###########################
+# Test Inverse Kinematics #
+###########################
+
+ADD_EXECUTABLE(TestInverseKinematics
+ ../src/portability/gettimeofday.cc
+ TestInverseKinematics.cpp
+ CommonTools.cpp
+ TestObject.cpp
+ ClockCPUTime.cpp
+)
+
+TARGET_LINK_LIBRARIES(TestInverseKinematics ${PROJECT_NAME})
+PKG_CONFIG_USE_DEPENDENCY(TestInverseKinematics jrl-dynamics)
+PKG_CONFIG_USE_DEPENDENCY(TestInverseKinematics hrp2-dynamics)
+ADD_DEPENDENCIES(TestInverseKinematics ${PROJECT_NAME})
+
+MESSAGE(STATUS "jrl data dir: " ${JRL_DYNAMICS_PKGDATAROOTDIR})
+SET(samplemodelpath ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/)
+SET(samplespec
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleSpecificities.xml
+)
+SET(sampleljr
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleLinkJointRank.xml
+)
+SET(sampleinitconfig
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleInitConfig.dat
+)
+
+LIST(APPEND LOGGING_WATCHED_VARIABLES samplespec sampleljr)
+
+# This test is disabled for now as it fails.
+# FIXME: fix the test and/or the implementation
+ADD_TEST(TestInverseKinematics
+TestInverseKinematics
+${samplemodelpath} sample.wrl ${samplespec} ${sampleljr} ${sampleinitconfig})
+
+
+######################
+# Test Hirukawa 2007 #
+######################
+
+ADD_EXECUTABLE(TestHirukawa2007
+../src/portability/gettimeofday.cc
+TestHirukawa2007.cpp
+CommonTools.cpp
+TestObject.cpp
+ClockCPUTime.cpp
+)
+TARGET_LINK_LIBRARIES(TestHirukawa2007 ${PROJECT_NAME})
+PKG_CONFIG_USE_DEPENDENCY(TestHirukawa2007 jrl-dynamics)
+PKG_CONFIG_USE_DEPENDENCY(TestHirukawa2007 hrp2-dynamics)
+ADD_DEPENDENCIES(TestHirukawa2007 ${PROJECT_NAME})
+
+MESSAGE(STATUS "jrl data dir: " ${JRL_DYNAMICS_PKGDATAROOTDIR})
+SET(samplemodelpath ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/)
+SET(samplespec
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleSpecificities.xml
+)
+SET(sampleljr
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleLinkJointRank.xml
+)
+SET(sampleinitconfig
+ ${JRL_DYNAMICS_PKGDATAROOTDIR}/jrl-dynamics/examples/data/sampleInitConfig.dat)
+
+LIST(APPEND LOGGING_WATCHED_VARIABLES samplespec sampleljr)
+
+# This test is disabled for now as it fails.
+# FIXME: fix the test and/or the implementation
+ADD_TEST(TestHirukawa2007
+ TestHirukawa2007
+ ${samplemodelpath} sample.wrl ${samplespec} ${sampleljr} ${sampleinitconfig})
####################
diff --git a/tests/CommonTools.cpp b/tests/CommonTools.cpp
index 160e0523976bfed7fa7b682f671b0cee1883a911..ffb44b5f746158a0caedaa484d9b47005ead46a9 100644
--- a/tests/CommonTools.cpp
+++ b/tests/CommonTools.cpp
@@ -140,6 +140,11 @@ namespace PatternGeneratorJRL {
SpecificitiesFileName = argv[3];
LinkJointRank = argv[4];
InitConfig = argv[5];
+ cout << VRMLPath << endl ;
+ cout << VRMLFileName << endl ;
+ cout << SpecificitiesFileName << endl ;
+ cout << LinkJointRank << endl ;
+ cout << InitConfig << endl ;
}
}
} /* End of TestSuite namespace */
diff --git a/tests/TestHirukawa2007.cpp b/tests/TestHirukawa2007.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5b0110154fb5130169e0f4ec7dae9a056798a8f
--- /dev/null
+++ b/tests/TestHirukawa2007.cpp
@@ -0,0 +1,590 @@
+/*
+ * Copyright 2010,
+ *
+ * Andrei Herdt
+ * Olivier Stasse
+ *
+ * JRL, CNRS/AIST
+ *
+ * This file is part of walkGenJrl.
+ * walkGenJrl 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.
+ *
+ * walkGenJrl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Lesser Public License for more details.
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Research carried out within the scope of the
+ * Joint Japanese-French Robotics Laboratory (JRL)
+ */
+/* \file This file tests A. Herdt's walking algorithm for
+ * automatic foot placement giving an instantaneous CoM velocity reference.
+ */
+#include "Debug.hh"
+#include "CommonTools.hh"
+#include "TestObject.hh"
+#include <hrp2-dynamics/hrp2OptHumanoidDynamicRobot.h>
+#include <ZMPRefTrajectoryGeneration/DynamicFilter.hh>
+#include <metapod/models/hrp2_14/hrp2_14.hh>
+#include <boost/fusion/algorithm/iteration/for_each.hpp>
+#include <boost/fusion/include/for_each.hpp>
+#include <boost/fusion/include/accumulate.hpp>
+#include <MultiContactRefTrajectoryGeneration/MultiContactHirukawa.hh>
+
+#ifndef METAPOD_TYPEDEF
+#define METAPOD_TYPEDEF
+ typedef double LocalFloatType;
+ typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
+ typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
+
+ typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::l_wrist >::type LhandNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::r_wrist >::type RhandNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::LARM_LINK0 >::type LshoulderNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::RARM_LINK0 >::type RshoulderNode;
+
+ typedef metapod::Nodes< Robot_Model, Robot_Model::LLEG_LINK0 >::type LhipNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::RLEG_LINK0 >::type RhipNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::l_ankle >::type LankleNode;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::r_ankle >::type RankleNode;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::l_ankle, Robot_Model::LLEG_LINK0,0,true,false> Jac_LF;
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_ankle, Robot_Model::RLEG_LINK0,0,true,false> Jac_RF;
+
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::l_wrist, Robot_Model::LARM_LINK0,0,true,false> Jac_LH;
+ typedef metapod::jac_point_chain < Robot_Model,
+ Robot_Model::r_wrist, Robot_Model::RARM_LINK0,0,true,false> Jac_RH;
+#endif
+
+typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
+
+using namespace::PatternGeneratorJRL;
+using namespace::PatternGeneratorJRL::TestSuite;
+using namespace std;
+
+typedef Eigen::Matrix<double,6,1> vector6d ;
+
+class TestHirukawa2007: public TestObject
+{
+
+private:
+
+ SimplePluginManager * SPM_ ;
+ double dInitX, dInitY;
+ bool once ;
+ MAL_VECTOR(InitialPosition,double);
+ double samplingPeriod ;
+
+ vector<COMState> comPos_ ;
+ vector< FootAbsolutePosition > rf_ ;
+ vector< FootAbsolutePosition > lf_ ;
+ vector< HandAbsolutePosition > rh_ ;
+ vector< HandAbsolutePosition > lh_ ;
+ vector<ZMPPosition> zmp_ ;
+
+ Robot_Model robot_ ;
+ Robot_Model::confVector q_init_ ;
+
+ vector< vector<double> > data_ ;
+
+ MultiContactHirukawa MCHpg ;
+
+public:
+ TestHirukawa2007(int argc, char *argv[], string &aString):
+ TestObject(argc,argv,aString)
+ {
+ SPM_ = NULL ;
+ once = true ;
+ MAL_VECTOR_RESIZE(InitialPosition,30);
+ samplingPeriod = 0.005 ;
+ }
+
+ ~TestHirukawa2007()
+ {
+ if ( SPM_ != 0 )
+ {
+ delete SPM_ ;
+ SPM_ = 0 ;
+ }
+ m_DebugHDR = 0;
+ }
+
+ typedef void (TestHirukawa2007::* localeventHandler_t)(PatternGeneratorInterface &);
+
+ struct localEvent
+ {
+ unsigned time;
+ localeventHandler_t Handler ;
+ };
+
+ bool doTest(ostream &os)
+ {
+ metapod::bcalc<Robot_Model>::run(robot_,q_init_);
+ metapod::jcalc<Robot_Model>::run(robot_,q_init_,Robot_Model::confVector::Zero());
+
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ oss.str("/home/mnaveau/devel/matlab_scripts/step_generator/testMetapod.txt");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+
+ boost::fusion::for_each(robot_.nodes , print_iXo() );
+
+ double sum_mass = 0.0 ;
+ metapod::Vector3dTpl< LocalFloatType >::Type com (0.0,0.0,0.0);
+
+ sum_mass = boost::fusion::accumulate(robot_.nodes , sum_mass , MassSum() );
+ com = boost::fusion::accumulate(robot_.nodes , com , MassbyComSum() );
+ cout << "mass * com = \n" << com << endl ;
+ cout << "mass = \n" << sum_mass << endl ;
+ com = com / sum_mass ;
+ cout << "com = \n" << com << endl ;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ data_.clear() ;
+ std::string astateFile =
+"/home/mnaveau/devel/ros_unstable/src/jrl/jrl-walkgen/_build-RELEASE/tests/TestMorisawa2007ShortWalk32TestFGPI.dat" ;
+ std::ifstream dataStream ;
+ dataStream.open(astateFile.c_str(),std::ifstream::in);
+
+ // reading all the data file
+ while (dataStream.good()) {
+ vector<double> oneLine(74) ;
+ for (unsigned int i = 0 ; i < oneLine.size() ; ++i)
+ dataStream >> oneLine[i];
+ data_.push_back(oneLine);
+ }
+ dataStream.close();
+
+ comPos_.resize(data_.size()) ;
+ rf_.resize(data_.size()) ;
+ lf_.resize(data_.size()) ;
+ rh_.resize(data_.size()) ;
+ lh_.resize(data_.size()) ;
+ zmp_ .resize(data_.size()) ;
+
+ for (unsigned int i = 0 ; i < data_.size() ; ++i)
+ {
+ comPos_[i].x[0] = data_[i][1] ;
+ comPos_[i].y[0] = data_[i][2] ;
+ comPos_[i].z[0] = data_[i][3] ;
+ comPos_[i].yaw[0] = data_[i][4] ;
+ comPos_[i].x[1] = data_[i][5] ;
+ comPos_[i].y[1] = data_[i][6] ;
+ comPos_[i].z[1] = data_[i][7] ;
+
+ rf_[i].x = data_[i][22] ;
+ rf_[i].y = data_[i][23] ;
+ rf_[i].z = data_[i][24] ;
+ rf_[i].omega = 0.0 ;
+ rf_[i].omega2 = 0.0 ;
+ rf_[i].theta = 0.0 ;
+
+ rf_[i].dx = data_[i][25] ;
+ rf_[i].dy = data_[i][26] ;
+ rf_[i].dz = data_[i][27] ;
+ rf_[i].domega = 0.0 ;
+ rf_[i].domega2 = 0.0 ;
+ rf_[i].dtheta = 0.0 ;
+
+ lf_[i].x = data_[i][10] ;
+ lf_[i].y = data_[i][11] ;
+ lf_[i].z = data_[i][12] ;
+ lf_[i].omega = 0.0 ;
+ lf_[i].omega2 = 0.0 ;
+ lf_[i].theta = 0.0 ;
+
+ lf_[i].dx = data_[i][13] ;
+ lf_[i].dy = data_[i][14] ;
+ lf_[i].dz = data_[i][15] ;
+ lf_[i].domega = 0.0 ;
+ lf_[i].domega2 = 0.0 ;
+ lf_[i].dtheta = 0.0 ;
+
+ rh_[i].dx = data_[i][4] ;
+ rh_[i].dy = data_[i][5] ;
+ rh_[i].dz = data_[i][6] ;
+ rh_[i].domega = 0.0 ;
+ rh_[i].domega2 = 0.0 ;
+ rh_[i].dtheta = 0.0 ;
+ rh_[i].stepType = 1.0 ;
+
+
+ lh_[i].dx = data_[i][4] ;
+ lh_[i].dy = data_[i][5] ;
+ lh_[i].dz = data_[i][6] ;
+ lh_[i].domega = 0.0 ;
+ lh_[i].domega2 = 0.0 ;
+ lh_[i].dtheta = 0.0 ;
+ lh_[i].stepType = 1.0 ;
+ }
+
+ MCHpg.online(comPos_,rf_,lf_,rh_,lh_) ;
+
+ vector<vector<int> > test_vector ;
+ test_vector.clear();
+ unsigned int dimension = 5 ;
+ test_vector.resize(dimension);
+ for (unsigned int i = 0 ; i < dimension ; ++i)
+ {
+ test_vector[i].clear();
+ for (unsigned int j = 0 ; j < i ; ++j)
+ {
+ int nbr = j ;
+ test_vector[i].push_back(nbr);
+ }
+ }
+
+
+ for (unsigned int i = 0 ; i < test_vector.size() ; ++i)
+ {
+ for (unsigned int j = 0 ; j < test_vector[i].size() ; ++j)
+ {
+ cout << test_vector[i][j] << " ";
+ }
+ cout << endl ;
+ }
+
+
+ //fillInDebugFiles();
+ return true ;
+ }
+
+ struct print_iXo
+ {
+ template <typename T>
+ void operator()(T & x) const
+ {
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ oss.str("/home/mnaveau/devel/matlab_scripts/step_generator/testMetapod.txt");
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(7);
+ aof.setf(ios::fixed, ios::floatfield);
+ aof << Robot_Model::inertias[x.id] ;
+ }
+ };
+
+ struct MassSum
+ {
+ typedef LocalFloatType result_type;
+
+ template <typename T>
+ result_type operator()( const result_type sum_mass, T & node) const
+ {
+ return ( sum_mass + Robot_Model::inertias[node.id].m() ) ;
+ }
+ };
+
+ struct MassbyComSum
+ {
+ typedef metapod::Vector3dTpl< LocalFloatType >::Type result_type;
+
+ template <typename T>
+ result_type operator()( const result_type sum_h, const T & x) const
+ {
+ double mass = Robot_Model::inertias[x.id].m() ;
+ return ( sum_h + mass * x.body.iX0.r() + x.body.iX0.E() * Robot_Model::inertias[x.id].h() );
+ }
+ };
+
+ void init()
+ {
+ // Instanciate and initialize.
+ string RobotFileName = m_VRMLPath + m_VRMLFileName;
+
+ bool fileExist = false;
+ {
+ std::ifstream file (RobotFileName.c_str ());
+ fileExist = !file.fail ();
+ }
+ if (!fileExist)
+ throw std::string ("failed to open robot model");
+
+ // Creating the humanoid robot.
+ SpecializedRobotConstructor(m_HDR);
+ if(m_HDR==0)
+ {
+ if (m_HDR!=0) delete m_HDR;
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ m_HDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+ }
+ // Parsing the file.
+ dynamicsJRLJapan::parseOpenHRPVRMLFile(*m_HDR,RobotFileName,
+ m_LinkJointRank,
+ m_SpecificitiesFileName);
+ // Create Pattern Generator Interface
+ m_PGI = patternGeneratorInterfaceFactory(m_HDR);
+
+ unsigned int lNbActuatedJoints = 30;
+ double * dInitPos = new double[lNbActuatedJoints];
+ ifstream aif;
+ aif.open(m_InitConfig.c_str(),ifstream::in);
+ if (aif.is_open())
+ {
+ for(unsigned int i=0;i<lNbActuatedJoints;i++)
+ aif >> dInitPos[i];
+ }
+ aif.close();
+
+ bool DebugConfiguration = true;
+ ofstream aofq;
+ if (DebugConfiguration)
+ {
+ aofq.open("TestConfiguration.dat",ofstream::out);
+ if (aofq.is_open())
+ {
+ for(unsigned int k=0;k<30;k++)
+ {
+ aofq << dInitPos[k] << " ";
+ }
+ aofq << endl;
+ }
+
+ }
+
+ // This is a vector corresponding to the DOFs actuated of the robot.
+ bool conversiontoradneeded=true;
+ if (conversiontoradneeded)
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
+ InitialPosition(i) = dInitPos[i]*M_PI/180.0;
+ else
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
+ InitialPosition(i) = dInitPos[i];
+
+ q_init_(0) = 0.0 ;
+ q_init_(1) = 0.0 ;
+ q_init_(2) = 0.6487 ;
+ q_init_(3) = 0.0 ;
+ q_init_(4) = 0.0 ;
+ q_init_(5) = 0.0 ;
+ for(unsigned int i=0 ; i<MAL_VECTOR_SIZE(InitialPosition) ; i++)
+ q_init_(i+6,0) = InitialPosition(i) ;
+
+ MCHpg.q(q_init_) ;
+
+ // This is a vector corresponding to ALL the DOFS of the robot:
+ // free flyer + actuated DOFS.
+ unsigned int lNbDofs = 36 ;
+ MAL_VECTOR_DIM(CurrentConfiguration,double,lNbDofs);
+ MAL_VECTOR_DIM(CurrentVelocity,double,lNbDofs);
+ MAL_VECTOR_DIM(CurrentAcceleration,double,lNbDofs);
+ MAL_VECTOR_DIM(PreviousConfiguration,double,lNbDofs) ;
+ MAL_VECTOR_DIM(PreviousVelocity,double,lNbDofs);
+ MAL_VECTOR_DIM(PreviousAcceleration,double,lNbDofs);
+ for(int i=0;i<6;i++)
+ {
+ PreviousConfiguration[i] = PreviousVelocity[i] = PreviousAcceleration[i] = 0.0;
+ }
+
+ for(unsigned int i=6;i<lNbDofs;i++)
+ {
+ PreviousConfiguration[i] = InitialPosition[i-6];
+ PreviousVelocity[i] = PreviousAcceleration[i] = 0.0;
+ }
+
+ delete [] dInitPos;
+
+ MAL_VECTOR_RESIZE(m_CurrentConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_CurrentVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_CurrentAcceleration, m_HDR->numberDof());
+
+ MAL_VECTOR_RESIZE(m_PreviousConfiguration, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_PreviousVelocity, m_HDR->numberDof());
+ MAL_VECTOR_RESIZE(m_PreviousAcceleration, m_HDR->numberDof());
+
+ SPM_ = new SimplePluginManager();
+ }
+
+protected:
+
+ void chooseTestProfile()
+ {return;}
+ void generateEvent()
+ {return;}
+
+ void SpecializedRobotConstructor(CjrlHumanoidDynamicRobot *& aHDR)
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ }
+
+// void fillInDebugFiles( )
+// {
+// /// \brief Create file .hip .pos .zmp
+// /// --------------------
+// ofstream aof;
+// string aFileName;
+// static int iteration = 0 ;
+
+// if ( iteration == 0 ){
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".pos";
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// }
+// ///----
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".pos";
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+// for(unsigned int i = 6 ; i < m_CurrentConfiguration.size() ; i++){
+// aof << filterprecision( m_CurrentConfiguration(i) ) << " " ; // 2
+// }
+// for(unsigned int i = 0 ; i < 9 ; i++){
+// aof << 0.0 << " " ;
+// }
+// aof << 0.0 << endl ;
+// aof.close();
+
+// if ( iteration == 0 ){
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".hip";
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// }
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".hip";
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+// aof << filterprecision( m_OneStep.finalCOMPosition.roll[0] * M_PI /180) << " " ; // 2
+// aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0] * M_PI /180 ) << " " ; // 3
+// aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] * M_PI /180 ) ; // 4
+// aof << endl ;
+// aof.close();
+
+// if ( iteration == 0 ){
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".zmp";
+// aof.open(aFileName.c_str(),ofstream::out);
+// aof.close();
+// }
+
+// FootAbsolutePosition aSupportState;
+// if (m_OneStep.LeftFootPosition.stepType < 0 )
+// aSupportState = m_OneStep.LeftFootPosition ;
+// else
+// aSupportState = m_OneStep.RightFootPosition ;
+
+// aFileName = "/opt/grx3.0/HRP2LAAS/etc/mnaveau/";
+// aFileName+=m_TestName;
+// aFileName+=".zmp";
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+// aof << filterprecision( m_OneStep.ZMPTarget(0) - m_CurrentConfiguration(0)) << " " ; // 2
+// aof << filterprecision( m_OneStep.ZMPTarget(1) - m_CurrentConfiguration(1) ) << " " ; // 3
+// aof << filterprecision( aSupportState.z - m_CurrentConfiguration(2)) ; // 4
+// aof << endl ;
+// aof.close();
+
+// aFileName = "/opt/grx3.0/HRP2LAAS/log/mnaveau/";
+// aFileName+="footpos";
+// aFileName+=".dat";
+// aof.open(aFileName.c_str(),ofstream::app);
+// aof.precision(8);
+// aof.setf(ios::scientific, ios::floatfield);
+// aof << filterprecision( iteration * 0.005 ) << " " ; // 1
+// aof << filterprecision( lfFoot[iteration].x ) << " " ; // 2
+// aof << filterprecision( lfFoot[iteration].y ) << " " ; // 3
+// aof << endl ;
+// aof.close();
+
+// iteration++;
+// }
+
+ void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
+ {
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ Chrp2OptHumanoidDynamicRobot *aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
+ aHDR = aHRP2HDR;
+ aDebugHDR = new Chrp2OptHumanoidDynamicRobot(&aRobotDynamicsObjectConstructor);
+ }
+
+ double filterprecision(double adb)
+ {
+ if (fabs(adb)<1e-7)
+ return 0.0;
+
+ double ladb2 = adb * 1e7;
+ double lintadb2 = trunc(ladb2);
+ return lintadb2/1e7;
+ }
+};
+
+int PerformTests(int argc, char *argv[])
+{
+#define NB_PROFILES 1
+ std::string TestNames = "TestHirukawa2007" ;
+
+ TestHirukawa2007 aTH2007(argc,argv,TestNames);
+ aTH2007.init();
+ try{
+ if (!aTH2007.doTest(std::cout)){
+ cout << "Failed test " << endl;
+ return -1;
+ }
+ else
+ cout << "Passed test " << endl;
+ }
+ catch (const char * astr){
+ cerr << "Failed on following error " << astr << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+int main(int argc, char *argv[])
+{
+ try
+ {
+ int ret = PerformTests(argc,argv);
+ return ret ;
+ }
+ catch (const std::string& msg)
+ {
+ std::cerr << msg << std::endl;
+ }
+ return 1;
+}
+
+
+
diff --git a/tests/TestInverseKinematics.cpp b/tests/TestInverseKinematics.cpp
index b04bf0e0af5f8d4e92ae190c8f96f7006138a737..273c7bc98b7ffeac0dfd471acca7462bf58fcac3 100644
--- a/tests/TestInverseKinematics.cpp
+++ b/tests/TestInverseKinematics.cpp
@@ -100,7 +100,7 @@ public:
int stage0 = 0 ;
int stage1 = 1 ;
double samplingPeriod = 0.005 ;
- dynamicfilter_->init( 0.0,
+ dynamicfilter_->init(
samplingPeriod,
samplingPeriod,
(double)(comPos.size()-320)*samplingPeriod,
@@ -268,7 +268,7 @@ public:
supportFoot = m_OneStep.RightFootPosition ;
}
double samplingPeriod = 0.005;
- dynamicfilter_->init(0.0,samplingPeriod,samplingPeriod,0.1,
+ dynamicfilter_->init(samplingPeriod,samplingPeriod,0.1,
1.6,0.814,supportFoot,m_OneStep.finalCOMPosition);
initIK();
MAL_VECTOR_TYPE(double) UpperConfig = m_HDR->currentConfiguration() ;
@@ -283,7 +283,6 @@ public:
m_CurrentVelocity,
m_CurrentAcceleration,
0.005,1,0);
- dynamicfilter_->InverseDynamics(m_CurrentConfiguration,m_CurrentVelocity,m_CurrentAcceleration);
}
protected:
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 99a82f057009350483a61460ce8530d098b6394c..c516357434c10449cb3af6e55b689dbbef123f30 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -181,7 +181,6 @@ public:
m_CurrentVelocity,
m_CurrentAcceleration,
0.005,1,iter);
- dynamicfilter_->InverseDynamics(m_CurrentConfiguration,m_CurrentVelocity,m_CurrentAcceleration);
iter++;
fillInDebugFiles();
}
@@ -245,7 +244,7 @@ public:
else{
supportFoot = m_OneStep.RightFootPosition ;
}
- dynamicfilter_->init(0.0,samplingPeriod_,samplingPeriod_,samplingPeriod_,
+ dynamicfilter_->init(samplingPeriod_,samplingPeriod_,samplingPeriod_,
samplingPeriod_,0.814,supportFoot,m_OneStep.finalCOMPosition);
initIK();
MAL_VECTOR_TYPE(double) UpperConfig = m_HDR->currentConfiguration() ;
@@ -260,7 +259,6 @@ public:
m_CurrentVelocity,
m_CurrentAcceleration,
0.005,1,0);
- dynamicfilter_->InverseDynamics(m_CurrentConfiguration,m_CurrentVelocity,m_CurrentAcceleration);
}
@@ -319,9 +317,9 @@ protected:
aof.setf(ios::scientific, ios::floatfield);
aof << filterprecision(m_OneStep.NbOfIt*samplingPeriod_ ) << " " // 1
<< filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
- << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
+ << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
<< filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
- << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
<< filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
<< filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
<< filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8
@@ -336,7 +334,7 @@ protected:
<< filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
<< filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
<< filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
- << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
<< filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 21
<< filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 22
<< filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23
@@ -704,20 +702,20 @@ protected:
aPGI.ParseCmd(strm2);
}
{
- istringstream strm2(":stepstairseq\
- 0.0 -0.105 0.0 0.0\
- 0.1 0.19 0.0 0.0\
- 0.1 -0.19 0.0 0.0\
- 0.1 0.19 0.0 0.0\
- 0.1 -0.19 0.0 0.0\
- 0.1 0.19 0.0 0.0\
- 0.1 -0.19 0.0 0.0\
- 0.1 0.19 0.0 0.0\
- 0.1 -0.19 0.0 0.0\
- 0.1 0.19 0.0 0.0\
- 0.1 -0.19 0.0 0.0\
- 0.0 0.19 0.0 0.0"
- );
+ istringstream strm2(":stepstairseq\
+ 0.0 -0.105 0.0 0.0\
+ 0.2 0.19 0.0 0.0\
+ 0.2 -0.19 0.0 0.0\
+ 0.2 0.19 0.0 0.0\
+ 0.2 -0.19 0.0 0.0\
+ 0.2 0.19 0.0 0.0\
+ 0.2 -0.19 0.0 0.0\
+ 0.2 0.19 0.0 0.0\
+ 0.2 -0.19 0.0 0.0\
+ 0.2 0.19 0.0 0.0\
+ 0.2 -0.19 0.0 0.0\
+ 0.0 0.19 0.0 0.0\
+ ");
aPGI.ParseCmd(strm2);
}
}
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index 4a7f97bbc6984dde5d30d50da83742d395895245..5a2ce25bc0809d38f9dbbb99a84e96e5dfdffd14 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -343,7 +343,7 @@ namespace PatternGeneratorJRL
<< filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
<< filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
<< filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
- << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
+ << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " " // 5
<< filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " " // 6
<< filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " " // 7
<< filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " " // 8