diff --git a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
index 99c63b31078e71d8ed4ee4d973a32ed94ebfeb0e..c64f995f1b32657b032585888f3695c9c0db2b95 100644
--- a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
+++ b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
@@ -483,25 +483,13 @@ namespace PatternGeneratorJRL
bool AnalyticalZMPCOGTrajectory::GetIntervalIndexFromTime(double t, unsigned int &j)
{
- ODEBUG("Here "<< m_DeltaTj.size());
- t -= m_AbsoluteTimeReference;
- double reftime=0;
- ODEBUG(" ====== CoM ====== ");
- for(unsigned int lj=0;lj<m_DeltaTj.size();lj++)
- {
- ODEBUG("t: " << t << " reftime :" << reftime << " Tj["<<lj << "]= "<< m_DeltaTj[lj]);
- if (((t+m_Sensitivity)>=reftime) && (t<=reftime+m_DeltaTj[lj]+m_Sensitivity))
- {
- j = lj;
- return true;
- }
- reftime+=m_DeltaTj[lj];
- }
- return false;
+ unsigned int prev_j = 0 ;
+ return GetIntervalIndexFromTime(t, j, prev_j);
}
bool AnalyticalZMPCOGTrajectory::GetIntervalIndexFromTime(double t, unsigned int &j, unsigned int &prev_j)
{
+ ODEBUG("Here "<< m_DeltaTj.size());
t -= m_AbsoluteTimeReference;
double reftime=0;
ODEBUG(" ====== CoM ====== ");
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index a84c3f949f23f76490c9efd7329f805261e1609d..a25c53b9b61aac530ea9d47dbc382b84b32128e1 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -642,16 +642,8 @@ namespace PatternGeneratorJRL
/*! Recompute time when a new step should be added. */
m_UpperTimeLimitToUpdateStacks = m_AbsoluteTimeReference + m_DeltaTj[0] + m_Tdble + 0.45 * m_Tsingle;
- FootAbsolutePosition supportFoot ;
- if (InitLeftFootAbsolutePosition.stepType<0)
- {
- supportFoot = InitLeftFootAbsolutePosition ;
- }else
- {
- supportFoot = InitRightFootAbsolutePosition ;
- }
m_kajitaDynamicFilter->init(m_CurrentTime, m_SamplingPeriod, 0.050, m_SamplingPeriod,
- 1.6, lStartingCOMState.z[0], supportFoot );
+ 1.6, lStartingCOMState.z[0], InitLeftFootAbsolutePosition );
return m_RelativeFootPositions.size();
}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index fca13b9dc23b410ae43f6cdb123204909d87acb6..c91be8f3b5926a5c29800bb6865a4909e63274b1 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -57,6 +57,8 @@ DynamicFilter::DynamicFilter(
DInitX_ = 0.0 ;
DInitY_ = 0.0 ;
+ jacobian_lf_ = Jacobian_LF::Jacobian::Zero();
+ jacobian_rf_ = Jacobian_RF::Jacobian::Zero();
}
DynamicFilter::~DynamicFilter()
@@ -79,7 +81,7 @@ void DynamicFilter::init(
double PG_T,
double previewWindowSize,
double CoMHeight,
- FootAbsolutePosition supportFoot)
+ FootAbsolutePosition inputLeftFoot)
{
currentTime_ = currentTime ;
controlPeriod_ = controlPeriod ;
@@ -87,13 +89,6 @@ void DynamicFilter::init(
PG_T_ = PG_T ;
previewWindowSize_ = previewWindowSize ;
- PreviousSupportFoot_(0,0) = supportFoot.x ;
- PreviousSupportFoot_(1,0) = supportFoot.y ;
- PreviousSupportFoot_(2,0) = supportFoot.z ;
- PreviousSupportFoot_(3,0) = supportFoot.omega ;
- PreviousSupportFoot_(4,0) = supportFoot.omega2 ;
- PreviousSupportFoot_(5,0) = supportFoot.theta ;
-
if (interpolationPeriod_>PG_T)
{NbI_=1;}
else
@@ -121,14 +116,20 @@ void DynamicFilter::init(
for(unsigned int j = 0 ; j < ZMPMBConfiguration_.size() ; j++ )
{
- m_q(j,0) = ZMPMBConfiguration_(j) ;
- m_dq(j,0) = ZMPMBVelocity_(j) ;
- m_ddq(j,0) = ZMPMBAcceleration_(j) ;
+ q_(j,0) = ZMPMBConfiguration_(j) ;
+ dq_(j,0) = ZMPMBVelocity_(j) ;
+ ddq_(j,0) = ZMPMBAcceleration_(j) ;
}
- m_prev_q = m_q ;
- m_prev_dq = m_dq ;
- m_prev_ddq = m_ddq ;
+ 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_ ;
+ bcalc<Robot_Model>::run(robot_, prev_q_);
deltaZMP_deq_.resize( PG_N_);
ZMPMB_vec_.resize( PG_N_, vector<double>(2));
@@ -182,12 +183,12 @@ int DynamicFilter::filter(
}
void DynamicFilter::InverseKinematics(
- COMState & lastCtrlCoMState,
- FootAbsolutePosition & lastCtrlLeftFoot,
- FootAbsolutePosition & lastCtrlRightFoot,
- deque<COMState> & inputCOMTraj_deq_,
- deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_)
+ const COMState & lastCtrlCoMState,
+ const FootAbsolutePosition & lastCtrlLeftFoot,
+ const FootAbsolutePosition & lastCtrlRightFoot,
+ const deque<COMState> & inputCOMTraj_deq_,
+ const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> & inputRightFootTraj_deq_)
{
int stage0 = 0 ;
int stage1 = 1 ;
@@ -209,9 +210,9 @@ void DynamicFilter::InverseKinematics(
}
void DynamicFilter::InverseKinematics(
- COMState & inputCoMState,
- FootAbsolutePosition & inputLeftFoot,
- FootAbsolutePosition & inputRightFoot,
+ const COMState & inputCoMState,
+ const FootAbsolutePosition & inputLeftFoot,
+ const FootAbsolutePosition & inputRightFoot,
MAL_VECTOR_TYPE(double)& configuration,
MAL_VECTOR_TYPE(double)& velocity,
MAL_VECTOR_TYPE(double)& acceleration,
@@ -231,7 +232,7 @@ void DynamicFilter::InverseKinematics(
aCoMAcc_(2) = inputCoMState.z[2]; aLeftFootPosition_(2) = inputLeftFoot.z;
aCoMAcc_(3) = inputCoMState.roll[2]; aLeftFootPosition_(3) = inputLeftFoot.theta;
aCoMAcc_(4) = inputCoMState.pitch[2];aLeftFootPosition_(4) = inputLeftFoot.omega;
- aCoMAcc_(5) = inputCoMState.yaw[2];
+ aCoMAcc_(5) = inputCoMState.yaw[2];bcalc<Robot_Model>::run(robot_, prev_q_);
aRightFootPosition_(0) = inputRightFoot.x;
aRightFootPosition_(1) = inputRightFoot.y;
@@ -272,9 +273,9 @@ void DynamicFilter::InverseDynamics(deque<ZMPPosition> inputZMPTraj_deq)
void DynamicFilter::ComputeZMPMB(
double samplingPeriod,
- COMState inputCoMState,
- FootAbsolutePosition inputLeftFoot,
- FootAbsolutePosition inputRightFoot,
+ const COMState & inputCoMState,
+ const FootAbsolutePosition & inputLeftFoot,
+ const FootAbsolutePosition & inputRightFoot,
vector<double> & ZMPMB,
int iteration)
{
@@ -283,70 +284,26 @@ void DynamicFilter::ComputeZMPMB(
ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_,
samplingPeriod, stage, iteration) ;
- Node & node_waist = boost::fusion::at_c<Robot_Model::BODY>(m_robot.nodes);
- Eigen::Matrix< LocalFloatType, 6, 1 > supportFoot ;
- TransformT<LocalFloatType,Spatial::RotationMatrixIdentityTpl<LocalFloatType> > supportFootXwaist;
- if (inputLeftFoot.stepType<0)
- {
- supportFoot(0,0) = inputLeftFoot.x ;
- supportFoot(1,0) = inputLeftFoot.y ;
- supportFoot(2,0) = inputLeftFoot.z ;
- supportFoot(3,0) = inputLeftFoot.omega ;
- supportFoot(4,0) = inputLeftFoot.omega2 ;
- supportFoot(5,0) = inputLeftFoot.theta ;
- Node & node_lleg0 = boost::fusion::at_c<Robot_Model::LLEG_LINK0>(m_robot.nodes);
- Node & node_lleg1 = boost::fusion::at_c<Robot_Model::LLEG_LINK1>(m_robot.nodes);
- Node & node_lleg2 = boost::fusion::at_c<Robot_Model::LLEG_LINK2>(m_robot.nodes);
- Node & node_lleg3 = boost::fusion::at_c<Robot_Model::LLEG_LINK3>(m_robot.nodes);
- Node & node_lleg4 = boost::fusion::at_c<Robot_Model::LLEG_LINK4>(m_robot.nodes);
- Node & node_l_ankle = boost::fusion::at_c<Robot_Model::l_ankle>(m_robot.nodes);
- supportFootXwaist = node_lleg0.sXp * node_lleg1.sXp * node_lleg2.sXp *
- node_lleg3.sXp * node_lleg4.sXp * node_l_ankle.sXp ;
- }else
- {
- supportFoot(0,0) = inputRightFoot.x ;
- supportFoot(1,0) = inputRightFoot.y ;
- supportFoot(2,0) = inputRightFoot.z ;
- supportFoot(3,0) = inputRightFoot.omega ;
- supportFoot(4,0) = inputRightFoot.omega2 ;
- supportFoot(5,0) = inputRightFoot.theta ;
- Node & node_rleg0 = boost::fusion::at_c<Robot_Model::RLEG_LINK0>(m_robot.nodes);
- Node & node_rleg1 = boost::fusion::at_c<Robot_Model::RLEG_LINK1>(m_robot.nodes);
- Node & node_rleg2 = boost::fusion::at_c<Robot_Model::RLEG_LINK2>(m_robot.nodes);
- Node & node_rleg3 = boost::fusion::at_c<Robot_Model::RLEG_LINK3>(m_robot.nodes);
- Node & node_rleg4 = boost::fusion::at_c<Robot_Model::RLEG_LINK4>(m_robot.nodes);
- Node & node_r_ankle = boost::fusion::at_c<Robot_Model::r_ankle>(m_robot.nodes);
- supportFootXwaist = node_rleg0.sXp * node_rleg1.sXp * node_rleg2.sXp *
- node_rleg3.sXp * node_rleg4.sXp * node_r_ankle.sXp ;
- }
-
- Eigen::Matrix< FloatType, 6, 1 > waist_pos , waist_speed, waist_acc ;
-
- waist_pos =
-
// Copy the angular trajectory data from "Boost" to "Eigen"
for(unsigned int j = 0 ; j < ZMPMBConfiguration_.size() ; j++ )
{
- m_q(j,0) = ZMPMBConfiguration_(j) ;
- m_dq(j,0) = ZMPMBVelocity_(j) ;
- m_ddq(j,0) = ZMPMBAcceleration_(j) ;
+ q_(j,0) = ZMPMBConfiguration_(j) ;
+ dq_(j,0) = ZMPMBVelocity_(j) ;
+ ddq_(j,0) = ZMPMBAcceleration_(j) ;
}
- // Apply the RNEA on the robot model
- metapod::rnea< Robot_Model, true >::run(m_robot, m_q, m_dq, m_ddq);
+ //computeWaist( inputLeftFoot );
+ // Apply the RNEA on the robot model
+ metapod::rnea< Robot_Model, true >::run(robot_, q_, dq_, ddq_);
+ RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
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] ;
- PreviousSupportFoot_ = supportFoot ;
- m_prev_q = m_q ;
- m_prev_dq = m_dq ;
- m_prev_ddq = m_ddq ;
-
return ;
}
@@ -359,15 +316,15 @@ void DynamicFilter::ComputeZMPMB(
// Copy the angular trajectory data from "Boost" to "Eigen"
for(unsigned int j = 0 ; j < configuration.size() ; j++ )
{
- m_q(j,0) = configuration(j) ;
- m_dq(j,0) = velocity(j) ;
- m_ddq(j,0) = acceleration(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(m_robot, m_q, m_dq, m_ddq);
+ metapod::rnea< Robot_Model, true >::run(robot_, q_, dq_, ddq_);
- Node & node = boost::fusion::at_c<Robot_Model::BODY>(m_robot.nodes);
+ RootNode & node = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
m_force = node.body.iX0.applyInv (node.joint.f);
ZMPMB.resize(2);
@@ -422,6 +379,54 @@ int DynamicFilter::OptimalControl(
return 0 ;
}
+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_)
+ {
+ Jacobian_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
+ {
+ Jacobian_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_ ;
+
+ return ;
+}
+
double DynamicFilter::filterprecision(double adb)
{
if (fabs(adb)<1e-7)
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index cd58c6f9f0cc0ad3a25577a95a02ef3ee32c5311..c24d24aa6a71cc734610f76b06ef1cabca01cb5c 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -4,13 +4,20 @@
// metapod includes
#include <metapod/models/hrp2_14/hrp2_14.hh>
#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
+#include <metapod/algos/jac_point_chain.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 Node;
+ typedef metapod::Nodes< Robot_Model, Robot_Model::BODY >::type RootNode;
+
+ 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;
+
#endif
namespace PatternGeneratorJRL
@@ -52,12 +59,12 @@ namespace PatternGeneratorJRL
/// \brief atomic function
void InverseKinematics(
- COMState & inputCoMState,
- FootAbsolutePosition & inputLeftFoot,
- FootAbsolutePosition & inputRightFoot,
- MAL_VECTOR_TYPE(double)& configuration,
- MAL_VECTOR_TYPE(double)& velocity,
- MAL_VECTOR_TYPE(double)& acceleration,
+ const COMState & inputCoMState,
+ const FootAbsolutePosition & inputLeftFoot,
+ const FootAbsolutePosition & inputRightFoot,
+ MAL_VECTOR_TYPE(double) & configuration,
+ MAL_VECTOR_TYPE(double) & velocity,
+ MAL_VECTOR_TYPE(double) & acceleration,
double samplingPeriod,
int stage,
int iteration);
@@ -65,9 +72,9 @@ namespace PatternGeneratorJRL
/// \brief atomic function allow to compute
void ComputeZMPMB(
double samplingPeriod,
- COMState inputCoMState,
- FootAbsolutePosition inputLeftFoot,
- FootAbsolutePosition inputRightFoot,
+ const COMState & inputCoMState,
+ const FootAbsolutePosition & inputLeftFoot,
+ const FootAbsolutePosition & inputRightFoot,
vector<double> & ZMPMB,
int iteration);
@@ -77,12 +84,12 @@ namespace PatternGeneratorJRL
/// \brief calculate, from the CoM computed by the preview control,
/// the corresponding articular position, velocity and acceleration
void InverseKinematics(
- COMState & lastCtrlCoMState,
- FootAbsolutePosition & lastCtrlLeftFoot,
- FootAbsolutePosition & lastCtrlRightFoot,
- deque<COMState> & inputCOMTraj_deq_,
- deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_);
+ const COMState & lastCtrlCoMState,
+ const FootAbsolutePosition & lastCtrlLeftFoot,
+ const FootAbsolutePosition & lastCtrlRightFoot,
+ const deque<COMState> & inputCOMTraj_deq_,
+ const deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ const deque<FootAbsolutePosition> & inputRightFootTraj_deq_);
/// \brief Apply the RNEA on the robot model and over the whole trajectory
/// given by the function "filter"
@@ -98,6 +105,8 @@ namespace PatternGeneratorJRL
/// \brief Preview control on the ZMPMBs computed
int OptimalControl(std::deque<COMState> & outputDeltaCOMTraj_deq_);
+ void computeWaist(const FootAbsolutePosition & inputLeftFoot);
+
// -------------------------------------------------------------------
/// \brief Debug function
@@ -207,22 +216,27 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) ZMPMBAcceleration_ ;
/// \brief data of the previous iteration
- Eigen::Matrix< LocalFloatType, 6, 1 > PreviousSupportFoot_ ;
- Robot_Model::confVector m_prev_q, m_prev_dq, m_prev_ddq;
+ bool PreviousSupportFoot_ ; // 1 = left ; 0 = right ;
+ Robot_Model::confVector prev_q_, prev_dq_, prev_ddq_;
/// \brief Inverse Dynamics variables
/// ---------------------------------
/// \brief Initialize the robot with the autogenerated files
/// by MetapodFromUrdf
- Robot_Model m_robot;
+ 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_ ;
/// \brief force acting on the CoM of the robot expressed
/// in the Euclidean Frame
Force_HRP2_14 m_force ;
/// \brief Set of configuration vectors (q, dq, ddq, torques)
- Robot_Model::confVector m_q, m_dq, m_ddq;
+ Robot_Model::confVector q_, dq_, ddq_;
/// \brief Used to eliminate the initiale difference between
/// the zmp and the zmpmb