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

algo/kinematics-derivatives: add support of LOCAL_WORLD_ALIGNED for joint velocity

parent aac17e4e
//
// Copyright (c) 2017-2019 CNRS INRIA
// Copyright (c) 2017-2020 CNRS INRIA
//
#ifndef __pinocchio_kinematics_derivatives_hxx__
......@@ -7,6 +7,7 @@
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
namespace pinocchio
{
......@@ -144,33 +145,53 @@ namespace pinocchio
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2;
Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv);
// dvec/dv
// dvec/dv: this result is then needed by dvec/dq
ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_);
if(rf == WORLD)
v_partial_dv_cols = Jcols;
else
motionSet::se3ActionInverse(oMlast,Jcols,v_partial_dv_cols);
switch(rf)
{
case WORLD:
v_partial_dv_cols = Jcols;
break;
case LOCAL_WORLD_ALIGNED:
details::translateJointJacobian(oMlast,Jcols,v_partial_dv_cols);
break;
case LOCAL:
motionSet::se3ActionInverse(oMlast,Jcols,v_partial_dv_cols);
break;
default:
assert(false && "This must never happened");
}
// dvec/dq
ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_);
if(rf == WORLD)
{
if(parent > 0)
vtmp = data.ov[parent] - vlast;
else
vtmp = -vlast;
motionSet::motionAction(vtmp,Jcols,v_partial_dq_cols);
}
else
switch(rf)
{
if(parent > 0)
{
vtmp = oMlast.actInv(data.ov[parent]);
case WORLD:
if(parent > 0)
vtmp = data.ov[parent] - vlast;
else
vtmp = -vlast;
motionSet::motionAction(vtmp,Jcols,v_partial_dq_cols);
break;
case LOCAL_WORLD_ALIGNED:
if(parent > 0)
vtmp = data.ov[parent] - vlast;
else
vtmp = -vlast;
vtmp.linear() += vtmp.angular().cross(oMlast.translation());
motionSet::motionAction(vtmp,v_partial_dv_cols,v_partial_dq_cols);
}
break;
case LOCAL:
if(parent > 0)
{
vtmp = oMlast.actInv(data.ov[parent]);
motionSet::motionAction(vtmp,v_partial_dv_cols,v_partial_dq_cols);
}
break;
default:
assert(false && "This must never happened");
}
}
};
......
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