Commit 2657164f authored by jcarpent's avatar jcarpent
Browse files

[C++][Python] Add acceleration related to gravity field

The idea is to separate the real spatial acceleration of a body to the
spatial acceleration containing the gravity field, which is useful in
the context of dynamical equations
parent 663b79b1
......@@ -64,10 +64,10 @@ namespace se3
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[(size_t) parent]);
data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.a[i] += data.liMi[i].actInv(data.a[(size_t) parent]);
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[(size_t) parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
}
};
......@@ -99,7 +99,7 @@ namespace se3
const Eigen::VectorXd & v)
{
data.v[0].setZero ();
data.a[0] = -model.gravity;
data.a_gf[0] = -model.gravity;
for( size_t i=1;i<(size_t) model.nbody;++i )
{
......
......@@ -67,10 +67,10 @@ namespace se3
data.v[(Model::Index)i] = jdata.v();
if(parent>0) data.v[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.v[parent]);
data.a[(Model::Index)i] = jdata.S()*jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ;
data.a[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.a[parent]);
data.a_gf[(Model::Index)i] = jdata.S()*jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ;
data.a_gf[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.a_gf[parent]);
data.f[(Model::Index)i] = model.inertias[(Model::Index)i]*data.a[(Model::Index)i] + model.inertias[(Model::Index)i].vxiv(data.v[(Model::Index)i]); // -f_ext
data.f[(Model::Index)i] = model.inertias[(Model::Index)i]*data.a_gf[(Model::Index)i] + model.inertias[(Model::Index)i].vxiv(data.v[(Model::Index)i]); // -f_ext
return 0;
}
......@@ -103,8 +103,8 @@ namespace se3
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
data.v[0] = Motion::Zero();
data.a[0] = -model.gravity;
data.v[0].setZero();
data.a_gf[0] = -model.gravity;
for( int i=1;i<model.nbody;++i )
{
......
......@@ -120,6 +120,7 @@ namespace se3
const Model& model;
JointDataVector joints;
std::vector<Motion> a; // Body acceleration
std::vector<Motion> a_gf; // Body acceleration with gravity
std::vector<Motion> v; // Body velocity
std::vector<Force> f; // Body force
std::vector<SE3> oMi; // Body absolute placement (wrt world)
......
......@@ -177,6 +177,7 @@ namespace se3
:model(ref)
,joints(0)
,a((std::size_t)ref.nbody)
,a_gf((std::size_t)ref.nbody)
,v((std::size_t)ref.nbody)
,f((std::size_t)ref.nbody)
,oMi((std::size_t)ref.nbody)
......@@ -225,6 +226,7 @@ namespace se3
a[0].setZero();
v[0].setZero();
a_gf[0] = -model.gravity;
f[0].setZero();
oMi[0].setIdentity();
liMi[0].setIdentity();
......
......@@ -70,6 +70,7 @@ namespace se3
cl
.ADD_DATA_PROPERTY(std::vector<Motion>,a,"Body acceleration")
.ADD_DATA_PROPERTY(std::vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
.ADD_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
.ADD_DATA_PROPERTY(std::vector<Force>,f,"Body force")
.ADD_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
......@@ -106,6 +107,7 @@ namespace se3
}
IMPL_DATA_PROPERTY(std::vector<Motion>,a,"Body acceleration")
IMPL_DATA_PROPERTY(std::vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
IMPL_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
IMPL_DATA_PROPERTY(std::vector<Force>,f,"Body force")
IMPL_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
......
......@@ -81,10 +81,13 @@ namespace se3
jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
if (parent > 0)
data.a[i] += data.liMi[i].actInv(data.a[parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
}
};
......@@ -144,8 +147,9 @@ namespace se3
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
data.v[0].setZero ();
data.a[0] = -model.gravity;
data.v[0].setZero();
data.a[0].setZero();
data.a_gf[0] = -model.gravity;
for(Model::Index i=1;i<(Model::Index) model.nbody;++i)
{
......
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