Commit 38cec6d5 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add algo to compute inefficiently the time derivative of the joint Jacobian

parent 36d2aa75
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -26,7 +26,7 @@ namespace se3
/// \brief Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.
/// The result is accessible through data.J.
///
/// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this Hacobian, it is then possible to easily extract the Jacobian of a specific joint frame.
/// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this Jacobian, it is then possible to easily extract the Jacobian of a specific joint frame.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
......@@ -71,6 +71,41 @@ namespace se3
const Eigen::VectorXd & q,
const Model::JointIndex jointId);
///
/// \brief Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depends both on q and v
/// The result is accessible through data.dJ.
///
///
/// \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).
///
/// \return The full model Jacobian (matrix 6 x model.nv).
///
inline const Data::Matrix6x &
computeJacobiansTimeVariation(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
///
/// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame or in the local frame of the joint.
/// \note This jacobian is extracted from data.dJ. You have to run se3::computeJacobiansTimeVariation before calling it.
///
/// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system.
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] jointId The id of the joint.
/// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.).
///
template<bool localFrame>
void getJacobian(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & dJ);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
......
......@@ -140,6 +140,91 @@ namespace se3
return data.J;
}
struct JacobiansTimeVariationForwardStep : public fusion::JointVisitor<JacobiansTimeVariationForwardStep>
{
typedef boost::fusion::vector <const se3::Model &,
se3::Data &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(JacobiansTimeVariationForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const se3::Model & model,
se3::Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::JointIndex & parent = model.parents[i];
SE3 & oMi = data.oMi[i];
Motion & vJ = data.v[i];
jmodel.calc(jdata.derived(),q,v);
vJ = jdata.v();
data.liMi[i] = model.jointPlacements[i]*jdata.M();
if(parent>0)
{
oMi = data.oMi[parent]*data.liMi[i];
vJ += data.liMi[i].actInv(data.v[parent]);
}
else
{
oMi = data.liMi[i];
}
jmodel.jointCols(data.J) = oMi.act(jdata.S());
// Spatial velocity of joint i expressed in the global frame o
const Motion ov(oMi.act(vJ));
jmodel.jointCols(data.dJ) = (ov.toActionMatrix() * jmodel.jointCols(data.J));
}
};
inline const Data::Matrix6x &
computeJacobiansTimeVariation(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
assert(model.check(data) && "data is not consistent with model.");
for(Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i)
{
JacobiansTimeVariationForwardStep::run(model.joints[i],data.joints[i],
JacobiansTimeVariationForwardStep::ArgsType(model,data,q,v));
}
return data.dJ;
}
template<bool localFrame>
void getJacobianTimeVariation(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & dJ)
{
assert( dJ.rows() == data.dJ.rows() );
assert( dJ.cols() == data.dJ.cols() );
assert(model.check(data) && "data is not consistent with model.");
const SE3 & oMjoint = data.oMi[jointId];
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
{
if(! localFrame ) dJ.col(j) = data.dJ.col(j);
else dJ.col(j) = oMjoint.actInv(Motion(data.dJ.col(j))).toVector();
}
}
} // namespace se3
/// @endcond
......
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