Commit 3e87918c authored by Justin Carpentier's avatar Justin Carpentier
Browse files

Merge pull request #26 from fvalenza/topic/JRU

[C++][Debug] Fixed operator * : Y*S in joint revolute unaligned
parents 4477e5ed 8758ecb5
......@@ -141,14 +141,14 @@ namespace se3
Eigen::Matrix<double,6,1>
operator*( const Inertia& Y,const JointRevoluteUnaligned::ConstraintRevoluteUnaligned & cru)
{
/* YS = [ m mcx ; -mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
/* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
const double &m = Y.mass();
const Inertia::Vector3 & c = Y.lever();
const Inertia::Symmetric3 & I = Y.inertia();
const Motion::Vector3 mcxw = m*c.cross(cru.axis);
Eigen::Matrix<double,6,1> res;
res.head<3>() = mcxw;
res.head<3>() = -mcxw;
res.tail<3>() = I*cru.axis - c.cross(mcxw);
return res;
}
......
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