Commit 8333ad13 authored by mnaveau's avatar mnaveau
Browse files

Corrected a bug in the dynamical filter of Kajita2003

Begin the implementation of a multicontact PG based on Hirukawa2007
parent 93e02c2a
......@@ -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
{
......
#include "MultiContactHirukawa.hh"
//#define VERBOSE
using namespace std ;
using namespace PatternGeneratorJRL ;
MultiContactHirukawa::MultiContactHirukawa()
{
n_samples_ = 1000; // size of data to treat
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::loadData()
int MultiContactHirukawa::InverseKinematicsOnLimbs(std::vector< FootAbsolutePosition > & rf,
std::vector< FootAbsolutePosition > & lf,
std::vector< HandAbsolutePosition > & rh,
std::vector< HandAbsolutePosition > & lh,
unsigned int currentIndex)
{
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();
v_com.resize(data_.size());
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::retrieveCoMandContact()
int MultiContactHirukawa::DetermineContact(std::vector< FootAbsolutePosition > & rf,
std::vector< FootAbsolutePosition > & lf,
std::vector< HandAbsolutePosition > & rh,
std::vector< HandAbsolutePosition > & lh)
{
return 0 ;
}
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) ;
}
}
int MultiContactHirukawa::InverseKinematicsOnLimbs()
{
#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 ;
}
......@@ -62,15 +129,14 @@ int MultiContactHirukawa::InverseMomentum()
return 0 ;
}
int MultiContactHirukawa::online()
int MultiContactHirukawa::online(vector<COMState> & comPos_,
vector<FootAbsolutePosition> & rf_,
vector<FootAbsolutePosition> & lf_,
vector<HandAbsolutePosition> & rh_,
vector<HandAbsolutePosition> & lh_)
{
loadData();
for (unsigned int i = 0 ; i < data_.size() ; ++i )
{ for (unsigned int j = 0 ; j < data_[0].size() ; ++j )
cout << data_[i][j] << " " ;
cout << endl ;
}
DetermineContact(rf_,lf_,rh_,lh_);
// InverseKinematicsOnLimbs(rf_,lf_,rh_,lh_,0);
return 0 ;
}
\ No newline at end of file
......@@ -3,35 +3,56 @@
#include <sstream>
#include <fstream>
#include <jrl/walkgen/pgtypes.hh>
#include <metapod/models/hrp2_14/hrp2_14.hh>
#include <metapod/algos/crba.hh>
#include <metapod/algos/jac_point_chain.hh>
#include "Clock.hh"
#include <metapod/tools/jcalc.hh>
# include <metapod/algos/jac.hh>
typedef double LocalFloatType;
typedef metapod::Spatial::ForceTpl<LocalFloatType> Force_HRP2_14;
typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
//#include "Clock.hh"
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;
#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::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::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::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::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_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 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
{
......@@ -40,26 +61,47 @@ public:
~MultiContactHirukawa();
int online();
int loadData();
int retrieveCoMandContact();
int InverseKinematicsOnLimbs();
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 * 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_ ;
//reading data
unsigned int n_samples_; // size of data to treat
unsigned int n_it_ ; // number of iteration max to converge
double sampling_period_ ; // sampling period in seconds
std::vector< std::vector<double> > data_ ; // data from planning
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
......@@ -590,7 +590,7 @@ computing the analytical trajectories. */
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
......@@ -683,8 +683,8 @@ computing the analytical trajectories. */
{
filteredCoM[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
filteredCoM[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] ;
COMStates[i].x[j] += outputDeltaCOMTraj_deq[i].x[j] ;
COMStates[i].y[j] += outputDeltaCOMTraj_deq[i].y[j] ;
}
m_kajitaDynamicFilter->ComputeZMPMB(m_SamplingPeriod, filteredCoM[i],
LeftFootAbsolutePositions[i], RightFootAbsolutePositions[i],
......
#include "DynamicFilter.hh"
#include <metapod/algos/rnea.hh>
#include <boost/fusion/include/for_each.hpp>
using namespace std;
using namespace PatternGeneratorJRL;
......@@ -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,20 +75,6 @@ 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)
......@@ -161,7 +148,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));
......@@ -213,18 +199,18 @@ int DynamicFilter::OffLinefilter(
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 ;
}
//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,
......@@ -237,6 +223,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];
......@@ -264,28 +252,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;
......@@ -297,8 +275,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;
......@@ -307,7 +283,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 ;
......@@ -321,9 +296,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
......@@ -405,36 +379,13 @@ void DynamicFilter::ComputeZMPMB(
m_force = node_waist.body.iX0.applyInv(node_waist.joint.f);
ZMPMB.resize(2);
ZMPMB[0] = - m_force.n()[1] / m_force.f()[2] ;
ZMPMB[1] = m_force.n()[0] / m_force.f()[2] ;
return ;
}
void DynamicFilter::ComputeZMPMB(
MAL_VECTOR_TYPE(double) & configuration,
MAL_VECTOR_TYPE(double) & velocity,
MAL_VECTOR_TYPE(double) & acceleration,
vector<double> & ZMPMB)
{
// Copy the angular trajectory data from "Boost" to "Eigen"
for(unsigned int j = 0 ; j < configuration.size() ; j++ )
{
q_(j,0) = configuration(j) ;
dq_(j,0) = velocity(j) ;
ddq_(j,0) = acceleration(j) ;
}
// Apply the RNEA on the robot model
metapod::rnea< Robot_Model, true >::run(robot_, q_, dq_, ddq_);
RootNode & node = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
m_force = node.body.iX0.applyInv (node.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() ;
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 ;
}
......@@ -443,20 +394,6 @@ int DynamicFilter::OptimalControl(
deque<ZMPPosition> & inputdeltaZMP_deq,
deque<COMState> & outputDeltaCOMTraj_deq_)
{
/// --------------------
ofstream aof;
string aFileName;
ostringstream oss(std::ostringstream::ate);
/// --------------------
oss.str("ZMPDiscretisationErr.dat");
aFileName = oss.str();
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);
if(!PC_->IsCoherent())
PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
......@@ -464,10 +401,6 @@ int DynamicFilter::OptimalControl(
// calcul of the preview control along the "deltaZMP_deq_"
for (unsigned i = 0 ; i < NCtrl_ ; i++ )
{
aof << i*controlPeriod_ << " " // 1
<< sxzmp_ << " " // 2
<< syzmp_ << " " // 3
<< endl ;
PC_->OneIterationOfPreview(deltax_,deltay_,