Commit 83c53ebd authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Reorganize pinocchio robot with m_qrpy and m_qpino.

Working version with Talos and dynamical filter.
Computation time needs to be checked.
parent a2da3bc9
......@@ -46,6 +46,12 @@ namespace PatternGeneratorJRL
};
typedef PinocchioRobotFoot_t PRFoot ;
namespace pinocchio_robot
{
const int RPY_SIZE=6;
const int QUATERNION_SIZE=7;
}
class PinocchioRobot
{
public:
......@@ -63,7 +69,8 @@ namespace PatternGeneratorJRL
Eigen::VectorXd & a);
void computeForwardKinematics();
void computeForwardKinematics(Eigen::VectorXd & q);
/// q should be given in RPY convention.
// void computeForwardKinematics(Eigen::VectorXd & q);
void RPYToSpatialFreeFlyer(Eigen::Vector3d & rpy,
Eigen::Vector3d & drpy,
......@@ -191,13 +198,18 @@ namespace PatternGeneratorJRL
inline pinocchio::JointModelVector & getActuatedJoints()
{return m_robotModel->joints;}
inline Eigen::VectorXd & currentConfiguration()
{return m_qmal;}
inline Eigen::VectorXd & currentVelocity()
{return m_vmal;}
inline Eigen::VectorXd & currentAcceleration()
{return m_amal;}
inline Eigen::VectorXd & currentPinoConfiguration()
{return m_qpino;}
inline Eigen::VectorXd & currentRPYConfiguration()
{return m_qrpy;}
inline Eigen::VectorXd & currentPinoVelocity()
{return m_vpino;}
inline Eigen::VectorXd & currentPinoAcceleration()
{return m_apino;}
inline Eigen::VectorXd & currentRPYVelocity()
{return m_vrpy;}
inline Eigen::VectorXd & currentRPYAcceleration()
{return m_arpy;}
inline unsigned numberDof()
{return m_robotModel->nq;}
......@@ -244,12 +256,16 @@ namespace PatternGeneratorJRL
/// SETTERS
/// ///////
inline void currentConfiguration(Eigen::VectorXd & conf)
{m_qmal=conf;}
inline void currentVelocity(Eigen::VectorXd & vel)
{m_vmal=vel;}
inline void currentAcceleration(Eigen::VectorXd & acc)
{m_amal=acc;}
void currentPinoConfiguration(Eigen::VectorXd & conf);
void currentRPYConfiguration(Eigen::VectorXd &);
inline void currentPinoVelocity(Eigen::VectorXd & vel)
{m_vpino=vel;}
inline void currentPinoAcceleration(Eigen::VectorXd & acc)
{m_apino=acc;}
inline void currentRPYVelocity(Eigen::VectorXd & vel)
{m_vrpy=vel;}
inline void currentRPYAcceleration(Eigen::VectorXd & acc)
{m_arpy=acc;}
inline pinocchio::JointIndex getFreeFlyerSize() const
{ return m_PinoFreeFlyerSize;}
......@@ -281,10 +297,22 @@ namespace PatternGeneratorJRL
pinocchio::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
pinocchio::JointIndex m_leftWrist , m_rightWrist ;
Eigen::VectorXd m_qmal ;
Eigen::VectorXd m_vmal ;
Eigen::VectorXd m_amal ;
/// Fields member to store configurations, velocity and acceleration
/// @{
/// Configuration SE(3) position + quaternion + NbDofs
Eigen::VectorXd m_qpino ;
/// Configuration SE(3) position + RPY + NbDofs
Eigen::VectorXd m_qrpy ;
/// Velocity se(3) + NbDofs
Eigen::VectorXd m_vpino ;
/// Velocity se(3) + NbDofs
Eigen::VectorXd m_vrpy ;
/// Acceleration acc + NbDofs
Eigen::VectorXd m_apino ;
Eigen::VectorXd m_arpy ;
// @}
// tmp variables
Eigen::Quaterniond m_quat ;
Eigen::Matrix3d m_rot ;
......
......@@ -309,7 +309,7 @@ InitializationHumanoid(Eigen::VectorXd &BodyAnglesIni,
// the model. But we do not use it.
// it is used to resize the temporary vector
PinocchioRobot *aPR = getPinocchioRobot();
Eigen::VectorXd CurrentConfig = aPR->currentConfiguration();
Eigen::VectorXd CurrentConfig = aPR->currentRPYConfiguration();
// Set to zero the free floating root.
if(lStartingWaistPose.size())
......@@ -329,17 +329,17 @@ InitializationHumanoid(Eigen::VectorXd &BodyAnglesIni,
// Initialize the configuration vector.
for(unsigned int i=0; i<BodyAnglesIni.size(); ++i)
{
CurrentConfig[i+aPR->getFreeFlyerSize()] = BodyAnglesIni[i];
CurrentConfig[i+pinocchio_robot::RPY_SIZE] =
BodyAnglesIni[i];
}
aPR->currentConfiguration(CurrentConfig);
aPR->currentRPYConfiguration(CurrentConfig);
// Compensate for the static translation, not the WAIST position
// but it is the body position which start on the ground.
aPR->computeForwardKinematics();
CurrentConfig = aPR->currentConfiguration();
CurrentConfig = aPR->currentRPYConfiguration();
// Set the waist position.
lStartingWaistPose(0) = CurrentConfig(0); // 0.0
......@@ -471,12 +471,12 @@ InitializationCoM
}
// Initialize previous configuration vector
{ for(unsigned int i=0;i<m_prev_Configuration.size();m_prev_Configuration[i++]=0.0);};
{ for(unsigned int i=0;i<m_prev_Configuration1.size();m_prev_Configuration1[i++]=0.0);};
{ for(unsigned int i=0;i<m_prev_Configuration2.size();m_prev_Configuration2[i++]=0.0);};
{ for(unsigned int i=0;i<m_prev_Velocity.size();m_prev_Velocity[i++]=0.0);};
{ for(unsigned int i=0;i<m_prev_Velocity1.size();m_prev_Velocity1[i++]=0.0);};
{ for(unsigned int i=0;i<m_prev_Velocity2.size();m_prev_Velocity2[i++]=0.0);};
m_prev_Configuration.setZero();
m_prev_Configuration1.setZero();
m_prev_Configuration2.setZero();
m_prev_Velocity.setZero();
m_prev_Velocity1.setZero();
m_prev_Velocity2.setZero();
for(unsigned int i = 0 ; i < BodyAnglesIni.size() ; ++i)
......@@ -1285,7 +1285,8 @@ void ComAndFootRealizationByGeometry::
ODEBUG5( qArml(0)<< " " << qArml(1) << " " << qArml(2) << " "
<< qArml(3) << " " << qArml(4) << " " << qArml(5) << " "
<< qArmr(0)<< " " << qArmr(1) << " " << qArmr(2) << " "
<< qArmr(3) << " " << qArmr(4) << " " << qArmr(5), "DebugDataqArmsHeuristic.txt");
<< qArmr(3) << " " << qArmr(4) << " " << qArmr(5),
"DebugDataqArmsHeuristic.txt");
}
bool ComAndFootRealizationByGeometry::
......
......@@ -674,11 +674,11 @@ namespace PatternGeneratorJRL {
(Eigen::VectorXd &Configuration,
Eigen::Vector3d &lStartingCOMState)
{
Eigen::VectorXd Velocity= m_PinocchioRobot->currentVelocity();
Eigen::VectorXd Velocity= m_PinocchioRobot->currentRPYVelocity();
Velocity.setZero();
m_PinocchioRobot->currentConfiguration(Configuration);
m_PinocchioRobot->currentVelocity(Velocity);
m_PinocchioRobot->currentRPYConfiguration(Configuration);
m_PinocchioRobot->currentRPYVelocity(Velocity);
m_PinocchioRobot->computeForwardKinematics();
m_PinocchioRobot->positionCenterOfMass(lStartingCOMState);
......@@ -1043,7 +1043,7 @@ namespace PatternGeneratorJRL {
Eigen::VectorXd lCurrentConfiguration;
lCurrentConfiguration = m_PinocchioRobot->currentConfiguration();
lCurrentConfiguration = m_PinocchioRobot->currentRPYConfiguration();
deque<RelativeFootPosition> lRelativeFootPositions;
CommonInitializationOfWalking
......@@ -1065,7 +1065,7 @@ namespace PatternGeneratorJRL {
lCurrentConfiguration(3) = 0.0;
lCurrentConfiguration(4) = 0.0;
lCurrentConfiguration(5) = 0.0;
m_PinocchioRobot->currentConfiguration(lCurrentConfiguration);
m_PinocchioRobot->currentRPYConfiguration(lCurrentConfiguration);
ODEBUG("Size of lRelativeFootPositions :"
<< lRelativeFootPositions.size());
......
......@@ -169,13 +169,13 @@ SetPreviewControl(PreviewControl *aPC)
aRightFootPosition(4) = aRightFAP.omega;
/* Get the current configuration vector */
CurrentConfiguration = m_PinocchioRobot->currentConfiguration();
CurrentConfiguration = m_PinocchioRobot->currentRPYConfiguration();
/* Get the current velocity vector */
CurrentVelocity = m_PinocchioRobot->currentVelocity();
CurrentVelocity = m_PinocchioRobot->currentRPYVelocity();
/* Get the current acceleration vector */
CurrentAcceleration = m_PinocchioRobot->currentAcceleration();
CurrentAcceleration = m_PinocchioRobot->currentRPYAcceleration();
m_ComAndFootRealization->
ComputePostureForGivenCoMAndFeetPosture
......@@ -191,13 +191,13 @@ SetPreviewControl(PreviewControl *aPC)
if (StageOfTheAlgorithm==0)
{
/* Update the current configuration vector */
m_PinocchioRobot->currentConfiguration(CurrentConfiguration);
m_PinocchioRobot->currentRPYConfiguration(CurrentConfiguration);
/* Update the current velocity vector */
m_PinocchioRobot->currentVelocity(CurrentVelocity);
m_PinocchioRobot->currentRPYVelocity(CurrentVelocity);
/* Update the current acceleration vector */
m_PinocchioRobot->currentAcceleration(CurrentAcceleration);
m_PinocchioRobot->currentRPYAcceleration(CurrentAcceleration);
}
}
......@@ -492,7 +492,7 @@ SetPreviewControl(PreviewControl *aPC)
ODEBUG("Stage 3");
Eigen::VectorXd CurrentConfiguration;
/* Get the current configuration vector */
CurrentConfiguration = m_PinocchioRobot->currentConfiguration();
CurrentConfiguration = m_PinocchioRobot->currentRPYConfiguration();
ODEBUG("Stage 4");
m_FIFODeltaZMPPositions.push_back(aZMPpos);
......@@ -511,9 +511,12 @@ SetPreviewControl(PreviewControl *aPC)
deque<FootAbsolutePosition> &RightFootPositions)
{
m_NumberOfIterations = 0;
Eigen::VectorXd CurrentConfiguration = m_PinocchioRobot->currentConfiguration();
Eigen::VectorXd CurrentVelocity = m_PinocchioRobot->currentVelocity();
Eigen::VectorXd CurrentAcceleration = m_PinocchioRobot->currentAcceleration();
Eigen::VectorXd CurrentConfiguration =
m_PinocchioRobot->currentRPYConfiguration();
Eigen::VectorXd CurrentVelocity =
m_PinocchioRobot->currentRPYVelocity();
Eigen::VectorXd CurrentAcceleration =
m_PinocchioRobot->currentRPYAcceleration();
m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITHOUT_INITIALPOS);
......@@ -595,14 +598,14 @@ SetPreviewControl(PreviewControl *aPC)
Eigen::VectorXd CurrentVelocity;
Eigen::VectorXd CurrentAcceleration;
/* Get the current configuration vector */
CurrentConfiguration = m_PinocchioRobot->currentConfiguration();
CurrentVelocity = m_PinocchioRobot->currentVelocity();
CurrentAcceleration = m_PinocchioRobot->currentAcceleration();
CurrentConfiguration = m_PinocchioRobot->currentRPYConfiguration();
CurrentVelocity = m_PinocchioRobot->currentRPYVelocity();
CurrentAcceleration = m_PinocchioRobot->currentRPYAcceleration();
CurrentVelocity.setZero();
CurrentAcceleration.setZero();
m_PinocchioRobot->currentVelocity(CurrentVelocity);
m_PinocchioRobot->currentAcceleration(CurrentAcceleration);
m_PinocchioRobot->currentRPYVelocity(CurrentVelocity);
m_PinocchioRobot->currentRPYAcceleration(CurrentAcceleration);
#ifdef _DEBUG_MODE_ON_
m_FIFOTmpZMPPosition.clear();
......
......@@ -38,20 +38,20 @@ PinocchioRobot::PinocchioRobot()
m_quat.normalize();
// resize by default
m_qmal.resize(50,1);
m_qmal.fill(0.0);
m_qmal(3)=m_quat.x();
m_qmal(4)=m_quat.y();
m_qmal(5)=m_quat.z();
m_qmal(6)=m_quat.w();
m_qpino.resize(50,1);
m_qpino.fill(0.0);
m_qpino(3)=m_quat.x();
m_qpino(4)=m_quat.y();
m_qpino(5)=m_quat.z();
m_qpino(6)=m_quat.w();
m_tau.resize(50,1);
m_vmal.fill(0.0);
m_amal.fill(0.0);
m_vpino.fill(0.0);
m_apino.fill(0.0);
m_tau.fill(0.0);
m_vmal.resize( 50 );
m_amal.resize( 50 );
m_vmal.Zero(50);
m_amal.Zero(50);
m_vpino.resize( 50 );
m_apino.resize( 50 );
m_vpino.Zero(50);
m_apino.Zero(50);
m_f.fill(0.0);
m_n.fill(0.0);
......@@ -192,16 +192,26 @@ initializeRobotModelAndData
m_robotDataInInitialePose = new pinocchio::Data(*m_robotModel);
m_robotDataInInitialePose->v[0] = pinocchio::Motion::Zero();
m_robotDataInInitialePose->a[0] = -m_robotModel->gravity;
m_qmal.resize(m_robotModel->nq);
m_qmal.setZero();
m_qmal[6]= 1.0 ;
m_vmal.resize(m_robotModel->nv);
m_vmal.setZero();
m_amal.resize(m_robotModel->nv);
m_amal.setZero();
m_qpino.resize(m_robotModel->nq);
m_qpino.setZero();
m_qrpy.resize(m_robotModel->nq-1);
m_qrpy.setZero();
m_qpino[6]= 1.0 ;
m_vpino.resize(m_robotModel->nv);
m_vpino.setZero();
m_vrpy.resize(m_robotModel->nv);
m_vrpy.setZero();
m_apino.resize(m_robotModel->nv);
m_apino.setZero();
m_arpy.resize(m_robotModel->nv);
m_arpy.setZero();
m_tau.resize(m_robotModel->nv);
m_tau.setZero();
pinocchio::forwardKinematics(*m_robotModel,*m_robotDataInInitialePose,m_qmal);
pinocchio::forwardKinematics
(*m_robotModel,*m_robotDataInInitialePose,m_qpino);
// compute the global mass of the robot
......@@ -448,38 +458,48 @@ RPYToSpatialFreeFlyer
void PinocchioRobot::computeForwardKinematics()
{
computeForwardKinematics(m_qmal);
pinocchio::forwardKinematics(*m_robotModel,*m_robotData,m_qpino);
pinocchio::centerOfMass(*m_robotModel,*m_robotData,m_qpino);
}
void PinocchioRobot::computeForwardKinematics(Eigen::VectorXd & q)
void PinocchioRobot::
currentPinoConfiguration
(Eigen::VectorXd &conf)
{
// euler to quaternion :
m_quat = Eigen::
Quaterniond
(Eigen::AngleAxisd(q(5), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(q(4), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(q(3), Eigen::Vector3d::UnitX()) ) ;
m_qpino = conf;
}
void PinocchioRobot::
currentRPYConfiguration
(Eigen::VectorXd &conf)
{
m_qrpy = conf;
m_quat = Eigen::Quaterniond
(Eigen::AngleAxisd(conf(5), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(conf(4), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(conf(3), Eigen::Vector3d::UnitX()) ) ;
m_quat.normalize();
// fill up m_q following the pinocchio standard : [pos quarternion DoFs]
for(unsigned i=0; i<3 ; ++i)
{
m_qmal(i) = q(i);
m_qpino(i) = conf(i);
}
m_qmal(3) = m_quat.x() ;
m_qmal(4) = m_quat.y() ;
m_qmal(5) = m_quat.z() ;
m_qmal(6) = m_quat.w() ;
for(int i=0; i<m_robotModel->nq-m_PinoFreeFlyerSize ; ++i)
// fill up m_q following the pinocchio standard : [pos quarternion DoFs]
m_qpino(3) = m_quat.x() ;
m_qpino(4) = m_quat.y() ;
m_qpino(5) = m_quat.z() ;
m_qpino(6) = m_quat.w() ;
for(unsigned i=6; i<conf.size() ; ++i)
{
m_qmal(m_PinoFreeFlyerSize+i) = q(m_PinoFreeFlyerSize+i);
}
pinocchio::forwardKinematics(*m_robotModel,*m_robotData,m_qmal);
pinocchio::centerOfMass(*m_robotModel,*m_robotData,m_qmal);
m_qpino(i+1) = conf(i);
}
}
void PinocchioRobot::computeInverseDynamics()
{
PinocchioRobot::computeInverseDynamics(m_qmal,m_vmal,m_amal);
PinocchioRobot::computeInverseDynamics(m_qrpy,m_vrpy,m_arpy);
}
void PinocchioRobot::
......@@ -497,33 +517,30 @@ computeInverseDynamics
// RPYToSpatialFreeFlyer(m_rpy,m_drpy,m_ddrpy,
// m_quat,m_omega,m_domega);
// euler to quaternion :
m_quat = Eigen::Quaterniond(
Eigen::AngleAxisd(q(5), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(q(4), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(q(3), Eigen::Vector3d::UnitX()) ) ;
m_quat = Eigen::Quaterniond
(Eigen::AngleAxisd(q(5), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(q(4), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(q(3), Eigen::Vector3d::UnitX()) ) ;
for(unsigned i=0; i<3 ; ++i)
{
m_qmal(i) = q(i);
m_qpino(i) = q(i);
}
m_rot = m_quat.toRotationMatrix().transpose() ;
m_vmal.segment<3>(0) = m_rot * m_vmal.segment<3>(0) ;
m_amal.segment<3>(0) = m_rot * m_amal.segment<3>(0) ;
m_vpino.segment<3>(0) = m_rot * m_vpino.segment<3>(0) ;
m_apino.segment<3>(0) = m_rot * m_apino.segment<3>(0) ;
// fill up m_q following the pinocchio standard : [pos quarternion DoFs]
m_qmal(3) = m_quat.x() ;
m_qmal(4) = m_quat.y() ;
m_qmal(5) = m_quat.z() ;
m_qmal(6) = m_quat.w() ;
m_qpino(3) = m_quat.x() ;
m_qpino(4) = m_quat.y() ;
m_qpino(5) = m_quat.z() ;
m_qpino(6) = m_quat.w() ;
// fill up the velocity and acceleration vectors
//m_v.segment<3>(3) = m_omega ;
//m_a.segment<3>(3) = m_domega ;
m_vmal = v;
m_amal = a;
m_vpino = v;
m_apino = a;
// performing the inverse dynamics
m_tau = pinocchio::rnea(*m_robotModel,*m_robotData,m_qmal,m_vmal,m_amal);
m_tau = pinocchio::rnea(*m_robotModel,*m_robotData,m_qpino,m_vpino,m_apino);
}
std::vector<pinocchio::JointIndex>
......@@ -550,8 +567,10 @@ std::vector<pinocchio::JointIndex> PinocchioRobot::jointsBetween
out.clear();
pinocchio::JointIndex lastCommonRank = 0 ;
pinocchio::JointIndex minChainLength =
fromRootToFirst.size() < fromRootToSecond.size()
? fromRootToFirst.size() : fromRootToSecond.size() ;
fromRootToFirst.size()
< fromRootToSecond.size()
? fromRootToFirst.size() :
fromRootToSecond.size() ;
for(unsigned k=1 ; k<minChainLength ; ++k)
{
......@@ -559,7 +578,8 @@ std::vector<pinocchio::JointIndex> PinocchioRobot::jointsBetween
++lastCommonRank;
}
for(std::vector<pinocchio::JointIndex>::size_type k=fromRootToFirst.size()-1;
for(std::vector<pinocchio::JointIndex>::size_type
k=fromRootToFirst.size()-1;
k>lastCommonRank ; --k)
{
out.push_back(fromRootToFirst[k]);
......@@ -568,7 +588,8 @@ std::vector<pinocchio::JointIndex> PinocchioRobot::jointsBetween
{
out.push_back(fromRootToSecond[0]);
}
for(pinocchio::JointIndex k=lastCommonRank+1 ; k<fromRootToSecond.size() ; ++k)
for(pinocchio::JointIndex k=lastCommonRank+1 ;
k<fromRootToSecond.size() ; ++k)
{
out.push_back(fromRootToSecond[k]);
}
......@@ -859,11 +880,13 @@ DetectAutomaticallyOneShoulder
FromRootToJoint.clear();
FromRootToJoint = fromRootToIt(aWrist);
std::vector<pinocchio::JointIndex>::iterator itJoint = FromRootToJoint.begin();
std::vector<pinocchio::JointIndex>::iterator itJoint =
FromRootToJoint.begin();
bool found=false;
while(itJoint!=FromRootToJoint.end())
{
std::vector<pinocchio::JointIndex>::iterator current = itJoint;
std::vector<pinocchio::JointIndex>::iterator
current = itJoint;
if (*current==m_chest)
found=true;
else
......
......@@ -614,9 +614,9 @@ computing the analytical trajectories. */
KajitaPCpreviewWindow,
lStartingCOMState );
/*! Set the upper body trajectory */
Eigen::VectorXd UpperConfig = m_PR->currentConfiguration() ;
Eigen::VectorXd UpperVel = m_PR->currentVelocity() ;
Eigen::VectorXd UpperAcc = m_PR->currentAcceleration() ;
Eigen::VectorXd UpperConfig = m_PR->currentRPYConfiguration() ;
Eigen::VectorXd UpperVel = m_PR->currentRPYVelocity() ;
Eigen::VectorXd UpperAcc = m_PR->currentRPYAcceleration() ;
// carry the weight in front of him
// UpperConfig(18)= 0.0 ; // CHEST_JOINT0
// UpperConfig(19)= 0.015 ; // CHEST_JOINT1
......@@ -658,7 +658,7 @@ computing the analytical trajectories. */
// UpperConfig(35)= 0.174532925 ; // LARM_JOINT6
for(unsigned int i = 18 ; i < 35 ; ++i){
UpperConfig(i)=m_PR->currentConfiguration()(i);
UpperConfig(i)=m_PR->currentRPYConfiguration()(i);
UpperVel(i)=0.0;
UpperAcc(i)=0.0;
}
......
......@@ -42,9 +42,9 @@ DynamicFilter::DynamicFilter(
deltay_.resize(3,1);
comAndFootRealization_->SetPreviousConfigurationStage0(
PR_->currentConfiguration());
PR_->currentRPYConfiguration());
comAndFootRealization_->SetPreviousVelocityStage0(
PR_->currentVelocity());
PR_->currentRPYVelocity());
sxzmp_.clear();
......@@ -161,6 +161,7 @@ void DynamicFilter::init(
zmpmb_i_.resize( (ZMPMB_vec_.size()-1)*inc +1);
deltaZMP_deq_.resize( (int)round( previewWindowSize_ / controlPeriod_ ));
/// Set CoM/LeftFoot/RightFoot/deltax/deltay sizes
aCoMState_.resize(6);
aCoMSpeed_.resize(6);
aCoMAcc_.resize(6);
......@@ -169,25 +170,27 @@ void DynamicFilter::init(
deltax_.resize(3,1);
deltay_.resize(3,1);
{ for(unsigned int i=0;i<aCoMState_.size();aCoMState_[i++]=0.0);};
{ for(unsigned int i=0;i<aCoMSpeed_.size();aCoMSpeed_[i++]=0.0);};
{ for(unsigned int i=0;i<aCoMAcc_.size();aCoMAcc_[i++]=0.0);};
{ for(unsigned int i=0;i<aLeftFootPosition_.size();aLeftFootPosition_[i++]=0.0);};
{ for(unsigned int i=0;i<aRightFootPosition_.size();aRightFootPosition_[i++]=0.0);};
{for(unsigned int i=0;i<deltax_.rows();i++) for(unsigned int j=0;j<deltax_.cols();j++) deltax_(i,j)=0.0;};
{for(unsigned int i=0;i<deltay_.rows();i++) for(unsigned int j=0;j<deltay_.cols();j++) deltay_(i,j)=0.0;};
upperPartConfiguration_ = PR_->currentConfiguration() ;
previousUpperPartConfiguration_ = PR_->currentConfiguration() ;
upperPartVelocity_ = PR_->currentVelocity() ;
previousUpperPartVelocity_ = PR_->currentVelocity() ;
upperPartAcceleration_ = PR_->currentAcceleration() ;
ZMPMBConfiguration_ = PR_->currentConfiguration() ;
ZMPMBVelocity_ = PR_->currentVelocity() ;
ZMPMBAcceleration_ = PR_->currentAcceleration() ;
previousZMPMBConfiguration_ = PR_->currentConfiguration() ;
previousZMPMBVelocity_ = PR_->currentVelocity() ;
/// Set CoM/LeftFoot/RightFoot/deltax/deltay to Zero
aCoMState_.setZero();
aCoMSpeed_.setZero();
aCoMAcc_.setZero();
aLeftFootPosition_.setZero();
aRightFootPosition_.setZero();
deltax_.setZero();
deltay_.setZero();
/// Update previous values
upperPartConfiguration_ = PR_->currentRPYConfiguration() ;
previousUpperPartConfiguration_ = PR_->currentRPYConfiguration() ;
upperPartVelocity_ = PR_->currentRPYVelocity() ;
previousUpperPartVelocity_ = PR_->currentRPYVelocity() ;
upperPartAcceleration_ = PR_->currentRPYAcceleration() ;
ZMPMBConfiguration_ = PR_->currentRPYConfiguration() ;
ZMPMBVelocity_ = PR_->currentRPYVelocity() ;
ZMPMBAcceleration_ = PR_->currentRPYAcceleration() ;
previousZMPMBConfiguration_ = PR_->currentRPYConfiguration() ;
previousZMPMBVelocity_ = PR_->currentRPYVelocity() ;
comAndFootRealization_->SetHeightOfTheCoM(CoMHeight_);