Skip to content
GitLab
Commit d3bccac9 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Update centerOfMass algo with a single function

parent b280437e
Hide whitespace changes
Inline Side-by-side
bindings/python/algorithm/expose-com.cpp
View file @ d3bccac9
......@@ -30,24 +30,18 @@ namespace se3
static SE3::Vector3
com_0_proxy(const Model& model,
Data & data,
const Eigen::VectorXd & q,
const bool updateKinematics = true)
const Eigen::VectorXd & q)
{
return centerOfMass(model,data,q,
true,
updateKinematics);
return centerOfMass(model,data,q,true);
}
static SE3::Vector3
com_1_proxy(const Model& model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const bool updateKinematics = true)
const Eigen::VectorXd & v)
{
return centerOfMass(model,data,q,v,
true,
updateKinematics);
return centerOfMass(model,data,q,v,true);
}
static SE3::Vector3
......@@ -55,27 +49,22 @@ namespace se3
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const bool updateKinematics = true)
const Eigen::VectorXd & a)
{
return centerOfMass(model,data,q,v,a,
true,
updateKinematics);
return centerOfMass(model,data,q,v,a,true);
}
void exposeCOM()
{
bp::def("centerOfMass",com_0_proxy,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Update kinematics"),
"Joint configuration q (size Model::nq)"),
"Compute the center of mass, putting the result in Data and return it.");
bp::def("centerOfMass",com_1_proxy,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
"Update kinematics"),
"Joint velocity v (size Model::nv)"),
"Computes the center of mass position and velocuty by storing the result in Data"
"and returns the center of mass position of the full model expressed in the world frame.");
......@@ -83,8 +72,7 @@ namespace se3
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
"Joint acceleration a (size Model::nv)",
"Update kinematics"),
"Joint acceleration a (size Model::nv)"),
"Computes the center of mass position, velocity and acceleration by storing the result in Data"
"and returns the center of mass position of the full model expressed in the world frame.");
......
bindings/python/scripts/robot_wrapper.py
View file @ d3bccac9
......@@ -67,14 +67,17 @@ class RobotWrapper(object):
def nv(self):
return self.model.nv
def com(self, q, v=None, a=None, update_kinematics=True):
def com(self, q=None, v=None, a=None):
if q is None:
se3.centerOfMass(self.model, self.data)
return data.com[0]
if v is not None:
if a is None:
se3.centerOfMass(self.model, self.data, q, v, update_kinematics)
se3.centerOfMass(self.model, self.data, q, v)
return self.data.com[0], self.data.vcom[0]
se3.centerOfMass(self.model, self.data, q, v, a, update_kinematics)
se3.centerOfMass(self.model, self.data, q, v, a)
return self.data.com[0], self.data.vcom[0], self.data.acom[0]
return se3.centerOfMass(self.model, self.data, q, update_kinematics)
return se3.centerOfMass(self.model, self.data, q)
def Jcom(self, q):
return se3.jacobianCenterOfMass(self.model, self.data, q)
......
src/algorithm/center-of-mass.hpp
View file @ d3bccac9
......@@ -33,27 +33,24 @@ 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 full rigid body system expressed in the world frame.
///
inline const SE3::Vector3 &
centerOfMass(const Model & model, Data & data,
const Eigen::VectorXd & q,
const bool computeSubtreeComs = true,
const bool updateKinematics = true);
const bool computeSubtreeComs = true);
///
/// \brief Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.
/// The result is accessible through data.com[0], data.vcom[0], data.acom[0] for the full body com position, velocity and acceleation.
/// And data.com[i], data.vcom[i] and data.acom[i] for the subtree supported by joint i (expressed in the joint i frame).
/// \brief Computes the center of mass position and velocity of a given model according to a particular joint configuration and velocity.
/// The result is accessible through data.com[0], data.vcom[0] for the full body com position and velocity.
/// And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] v The joint velocity 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.
///
/// \return The center of mass position of the full rigid body system expressed in the world frame.
///
......@@ -61,8 +58,7 @@ namespace se3
centerOfMass(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const bool computeSubtreeComs = true,
const bool updateKinematics = true);
const bool computeSubtreeComs = true);
///
/// \brief Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.
......@@ -75,7 +71,6 @@ 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.
///
/// \return The center of mass position of the full rigid body system expressed in the world frame.
///
......@@ -84,8 +79,37 @@ namespace se3
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const bool computeSubtreeComs = true,
const bool updateKinematics = true);
const bool computeSubtreeComs = true);
///
/// \brief Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data and the template value parameters.
/// The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity.
/// And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).
///
/// \tparam do_position Compute the center of mass position.
/// \tparam do_velocity Compute the center of mass velocity.
/// \tparam do_acceleration Compute the center of mass acceleration.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
///
template<bool do_position, bool do_velocity, bool do_acceleration>
inline void centerOfMass(const Model & model, Data & data,
const bool computeSubtreeComs = true);
///
/// \brief Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data.
/// The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity.
/// And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees.
///
inline void centerOfMass(const Model & model, Data & data,
const bool computeSubtreeComs)
{ centerOfMass<true,true,true>(model,data,computeSubtreeComs); }
///
/// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
......
src/algorithm/center-of-mass.hxx
View file @ d3bccac9
......@@ -24,107 +24,26 @@
namespace se3
{
inline const SE3::Vector3 &
centerOfMass(const Model & model, Data & data,
const Eigen::VectorXd & q,
const bool computeSubtreeComs,
const bool updateKinematics)
const bool computeSubtreeComs)
{
assert(model.check(data) && "data is not consistent with model.");
data.mass[0] = 0;
data.com[0].setZero ();
// Forward Step
if (updateKinematics)
forwardKinematics(model, data, q);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
data.com[i] = mass * lever;
data.mass[i] = mass;
}
// Backward Step
for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1); i>0; --i)
{
const Model::JointIndex & 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];
}
}
forwardKinematics(model,data,q);
data.com[0] /= data.mass[0];
centerOfMass(model,data,computeSubtreeComs);
return data.com[0];
}
inline const SE3::Vector3 &
centerOfMass(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const bool computeSubtreeComs,
const bool updateKinematics)
const bool computeSubtreeComs)
{
assert(model.check(data) && "data is not consistent with model.");
using namespace se3;
data.mass[0] = 0;
data.com[0].setZero ();
data.vcom[0].setZero ();
// Forward Step
if (updateKinematics)
forwardKinematics(model, data, q, v);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
const Motion & v = data.v[i];
data.com[i] = mass * lever;
data.mass[i] = mass;
data.vcom[i] = mass * (v.angular().cross(lever) + v.linear());
}
// Backward Step
for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1); i>0; --i)
{
const Model::JointIndex & parent = model.parents[i];
const SE3 & liMi = data.liMi[i];
data.com[parent] += (liMi.rotation()*data.com[i]
+ data.mass[i] * liMi.translation());
data.vcom[parent] += liMi.rotation()*data.vcom[i];
data.mass[parent] += data.mass[i];
if( computeSubtreeComs )
{
data.com[i] /= data.mass[i];
data.vcom[i] /= data.mass[i];
}
}
data.com[0] /= data.mass[0];
data.vcom[0] /= data.mass[0];
forwardKinematics(model,data,q,v);
centerOfMass<true,true,false>(model,data,computeSubtreeComs);
return data.com[0];
}
......@@ -133,21 +52,30 @@ namespace se3
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const bool computeSubtreeComs,
const bool updateKinematics)
const bool computeSubtreeComs)
{
forwardKinematics(model,data,q,v,a);
centerOfMass<true,true,true>(model,data,computeSubtreeComs);
return data.com[0];
}
template<bool do_position, bool do_velocity, bool do_acceleration>
inline void centerOfMass(const Model & model, Data & data,
const bool computeSubtreeComs)
{
assert(model.check(data) && "data is not consistent with model.");
using namespace se3;
data.mass[0] = 0;
data.com[0].setZero ();
data.vcom[0].setZero ();
data.acom[0].setZero ();
if(do_position)
data.com[0].setZero ();
if(do_velocity)
data.vcom[0].setZero ();
if(do_acceleration)
data.acom[0].setZero ();
// Forward Step
if (updateKinematics)
forwardKinematics(model, data, q, v, a);
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
const double mass = model.inertias[i].mass();
......@@ -156,40 +84,52 @@ namespace se3
const Motion & v = data.v[i];
const Motion & a = data.a[i];
data.com[i] = mass * lever;
data.mass[i] = mass;
if(do_position)
data.com[i] = mass * lever;
data.vcom[i] = mass * (v.angular().cross(lever) + v.linear());
data.acom[i] = mass * (a.angular().cross(lever) + a.linear()) + v.angular().cross(data.vcom[i]); // take into accound the coriolis part of the acceleration
if(do_velocity)
data.vcom[i] = mass * (v.angular().cross(lever) + v.linear());
if(do_acceleration)
data.acom[i] = mass * (a.angular().cross(lever) + a.linear())
+ v.angular().cross(data.vcom[i]); // take into accound the coriolis part of the acceleration
}
// Backward Step
for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1); i>0; --i)
{
const Model::JointIndex & 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(do_position)
data.com[parent] += (liMi.rotation()*data.com[i]
+ data.mass[i] * liMi.translation());
data.vcom[parent] += liMi.rotation()*data.vcom[i];
if(do_velocity)
data.vcom[parent] += liMi.rotation()*data.vcom[i];
data.acom[parent] += liMi.rotation()*data.acom[i];
data.mass[parent] += data.mass[i];
if( computeSubtreeComs )
if(computeSubtreeComs)
{
data.com[i] /= data.mass[i];
data.vcom[i] /= data.mass[i];
data.acom[i] /= data.mass[i];
if(do_position)
data.com[i] /= data.mass[i];
if(do_velocity)
data.vcom[i] /= data.mass[i];
if(do_acceleration)
data.acom[i] /= data.mass[i];
}
}
data.com[0] /= data.mass[0];
data.vcom[0] /= data.mass[0];
data.acom[0] /= data.mass[0];
return data.com[0];
if(do_position)
data.com[0] /= data.mass[0];
if(do_velocity)
data.vcom[0] /= data.mass[0];
if(do_acceleration)
data.acom[0] /= data.mass[0];
}
inline const SE3::Vector3 &
......
unittest/compute-all-terms.cpp
View file @ d3bccac9
......@@ -61,7 +61,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
getJacobianComFromCrba(model, data_other);
centerOfMass(model, data_other, q, v, true, true);
centerOfMass(model, data_other, q, v, true);
kineticEnergy(model, data_other, q, v, true);
potentialEnergy(model, data_other, q, true);
......@@ -91,7 +91,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
getJacobianComFromCrba(model, data_other);
centerOfMass(model, data_other, q, v, true, true);
centerOfMass(model, data_other, q, v, true);
kineticEnergy(model, data_other, q, v, true);
potentialEnergy(model, data_other, q, true);
......@@ -122,7 +122,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
getJacobianComFromCrba(model, data_other);
centerOfMass(model, data_other, q, v, true, true);
centerOfMass(model, data_other, q, v, true);
kineticEnergy(model, data_other, q, v, true);
potentialEnergy(model, data_other, q, true);
......@@ -153,7 +153,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
getJacobianComFromCrba(model, data_other);
centerOfMass(model, data_other, q, v, true, true);
centerOfMass(model, data_other, q, v, true);
kineticEnergy(model, data_other, q, v, true);
potentialEnergy(model, data_other, q, true);
......
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