Verified Commit e5d52188 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo: compute oinertias in derivatives algos

parent d01c0677
......@@ -55,7 +55,7 @@ namespace pinocchio
ov = data.oMi[i].act(data.v[i]);
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.Yaba[i] = model.inertias[i].matrix();
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
data.oh[i] = data.oYcrb[i] * ov;
data.of[i] = ov.cross(data.oh[i]);
......
//
// Copyright (c) 2017-2019 CNRS INRIA
// Copyright (c) 2017-2020 CNRS INRIA
//
#ifndef __pinocchio_rnea_derivatives_hxx__
......@@ -46,7 +46,7 @@ namespace pinocchio
else
data.oMi[i] = data.liMi[i];
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
data.of[i] = data.oYcrb[i] * minus_gravity;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
......@@ -242,7 +242,7 @@ namespace pinocchio
data.a[i] += data.liMi[i].actInv(data.a[parent]);
}
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
ov = data.oMi[i].act(data.v[i]);
oa = data.oMi[i].act(data.a[i]);
oa_gf = oa - model.gravity; // add gravity contribution
......
Markdown is supported
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