Commit 663b79b1 authored by jcarpent's avatar jcarpent
Browse files

[C++] Add option to effectively updates or not the kinematics of the model

parent 8ac35aa8
......@@ -33,13 +33,15 @@ namespace se3
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
/// \param[in] updateKinematics If true, the algorithm updates first the geometry of the tree. Otherwise, it uses the current kinematics stored in data.
///
/// \return The center of mass position of the rigid body system expressed in the world frame (vector 3).
///
inline const Eigen::Vector3d &
centerOfMass(const Model & model, Data& data,
const Eigen::VectorXd & q,
const bool & computeSubtreeComs = true);
const bool computeSubtreeComs = true,
const bool updateKinematics = true);
///
/// \brief Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.
......@@ -52,12 +54,14 @@ namespace se3
/// \param[in] v The joint velocity vector (dim model.nv).
/// \param[in] a The joint acceleration vector (dim model.nv).
/// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
/// \param[in] updateKinematics If true, the algorithm updates first the second order kinematics of the tree. Otherwise, it uses the current kinematics stored in data.
///
inline void centerOfMassAcceleration(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const bool & computeSubtreeComs = true);
const bool computeSubtreeComs = true,
const bool updateKinematics = true);
///
/// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
......@@ -74,7 +78,7 @@ namespace se3
inline const Eigen::Matrix<double,3,Eigen::Dynamic> &
jacobianCenterOfMass(const Model & model, Data & data,
const Eigen::VectorXd & q,
const bool & computeSubtreeComs = true);
const bool computeSubtreeComs = true);
/* If the CRBA has been run, then both COM and Jcom are easily available from
* the mass matrix. Use the following methods to access them. In that case,
......
......@@ -21,80 +21,57 @@
namespace se3
{
struct CenterOfMassForwardStep : public fusion::JointVisitor<CenterOfMassForwardStep>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Eigen::VectorXd &,
const bool &
> ArgsType;
JOINT_VISITOR_INIT(CenterOfMassForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Eigen::VectorXd & q,
const bool & computeSubtreeComs)
{
using namespace Eigen;
using namespace se3;
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
jmodel.calc(jdata.derived(),q);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.com[parent] += (data.liMi[i].rotation()*data.com[i]
+data.mass[i]*data.liMi[i].translation());
data.mass[parent] += data.mass[i];
if( computeSubtreeComs )
data.com[i] /= data.mass[i];
}
};
/* Compute the centerOfMass in the local frame of the root joint. */
inline const Eigen::Vector3d &
centerOfMass(const Model & model, Data & data,
const Eigen::VectorXd & q,
const bool & computeSubtreeComs)
const bool computeSubtreeComs,
const bool updateKinematics)
{
data.mass[0] = 0;
data.com[0].setZero ();
// Forward Step
if (updateKinematics)
forwardKinematics(model, data, q);
for( Model::Index i=1;i<(Model::Index)(model.nbody);++i )
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
{
data.com[i] = model.inertias[i].mass()*model.inertias[i].lever();
data.mass[i] = model.inertias[i].mass();
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
data.com[i] = mass * lever;
data.mass[i] = mass;
}
for( Model::Index i=(Model::Index)(model.nbody-1);i>0;--i )
// Backward Step
for(Model::Index i=(Model::Index)(model.nbody-1); i>0; --i)
{
CenterOfMassForwardStep
::run(model.joints[i],data.joints[i],
CenterOfMassForwardStep::ArgsType(model,data,q,computeSubtreeComs));
const Model::Index & parent = model.parents[i];
const SE3 & liMi = data.liMi[i];
data.com[parent] += (liMi.rotation()*data.com[i]
+ data.mass[i] * liMi.translation());
data.mass[parent] += data.mass[i];
if(computeSubtreeComs)
{
data.com[i] /= data.mass[i];
}
}
data.com[0] /= data.mass[0];
return data.com[0];
}
/* Compute the centerOfMass position, velocity and acceleration in the local frame of the root joint. */
inline void
centerOfMassAcceleration(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const bool & computeSubtreeComs)
const bool computeSubtreeComs,
const bool updateKinematics)
{
using namespace se3;
......@@ -104,7 +81,9 @@ namespace se3
data.acom[0].setZero ();
// Forward Step
forwardKinematics(model, data, q, v, a);
if (updateKinematics)
forwardKinematics(model, data, q, v, a);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
{
const double mass = model.inertias[i].mass();
......@@ -195,7 +174,7 @@ namespace se3
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const bool &
const bool
> ArgsType;
JOINT_VISITOR_INIT(JacobianCenterOfMassBackwardStep);
......@@ -205,7 +184,7 @@ namespace se3
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model& model,
se3::Data& data,
const bool & computeSubtreeComs )
const bool computeSubtreeComs )
{
using namespace Eigen;
using namespace se3;
......@@ -238,7 +217,7 @@ namespace se3
inline const Eigen::Matrix<double,3,Eigen::Dynamic> &
jacobianCenterOfMass(const Model & model, Data & data,
const Eigen::VectorXd & q,
const bool & computeSubtreeComs)
const bool computeSubtreeComs)
{
data.com[0].setZero ();
data.mass[0] = 0;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment