Commit e9bb3637 authored by jcarpent's avatar jcarpent
Browse files

[All] Update function name when considering joint Jacobian

Some functions working on Jacobian have been renamed:

- getJacobian is now getJointJacobian
- computeJacobians is now computeJointJacobians
- getJacobianTimeVariation is now getJointJacobianTimeVariation
- computeJacobiansTimeVariation is now getJointJacobianTimeVariation
parent 6be56007
......@@ -151,14 +151,14 @@ int main(int argc, const char ** argv)
timer.tic();
SMOOTH(NBT)
{
computeJacobians(model,data,qs[_smooth]);
computeJointJacobians(model,data,qs[_smooth]);
}
std::cout << "Jacobian = \t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
computeJacobiansTimeVariation(model,data,qs[_smooth],qdots[_smooth]);
computeJointJacobiansTimeVariation(model,data,qs[_smooth],qdots[_smooth]);
}
std::cout << "Jacobian Time Variation = \t"; timer.toc(std::cout,NBT);
......
......@@ -43,7 +43,7 @@ namespace se3
const bool local,
const Eigen::VectorXd & q)
{
computeJacobians(model,data,q);
computeJointJacobians(model,data,q);
framesForwardKinematics(model,data);
return frame_jacobian_proxy(model, data, frame_id, local);
......@@ -85,7 +85,7 @@ namespace se3
"The columns of the Jacobian are expressed in the frame coordinates.\n"
"In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
"where v is the time derivative of the configuration q.\n"
"Be aware that computeJacobians and framesKinematics must have been called first.");
"Be aware that computeJointJacobians and framesKinematics must have been called first.");
}
} // namespace python
......
......@@ -34,10 +34,10 @@ namespace se3
Data::Matrix6x J(6,model.nv); J.setZero();
if (update_kinematics)
computeJacobians(model,data,q);
computeJointJacobians(model,data,q);
if(local) getJacobian<LOCAL> (model,data,jointId,J);
else getJacobian<WORLD> (model,data,jointId,J);
if(local) getJointJacobian<LOCAL>(model,data,jointId,J);
else getJointJacobian<WORLD>(model,data,jointId,J);
return J;
}
......@@ -50,8 +50,8 @@ namespace se3
{
Data::Matrix6x J(6,model.nv); J.setZero();
if(local) getJacobian<LOCAL> (model,data,jointId,J);
else getJacobian<WORLD> (model,data,jointId,J);
if(local) getJointJacobian<LOCAL>(model,data,jointId,J);
else getJointJacobian<WORLD>(model,data,jointId,J);
return J;
}
......@@ -64,23 +64,22 @@ namespace se3
{
Data::Matrix6x dJ(6,model.nv); dJ.setZero();
if(local) getJacobianTimeVariation<LOCAL> (model,data,jointId,dJ);
else getJacobianTimeVariation<WORLD> (model,data,jointId,dJ);
if(local) getJointJacobianTimeVariation<LOCAL>(model,data,jointId,dJ);
else getJointJacobianTimeVariation<WORLD>(model,data,jointId,dJ);
return dJ;
}
void exposeJacobian()
{
bp::def("computeJacobians",(const Data::Matrix6x &(*)(const Model &, Data &, const Eigen::VectorXd &))&computeJacobians,
bp::def("computeJacobians",(const Data::Matrix6x &(*)(const Model &, Data &, const Eigen::VectorXd &))&computeJointJacobians,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)"),
"Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.\n"
"The result is accessible through data.J. This function computes also the forwardKinematics of the model.",
bp::return_value_policy<bp::return_by_value>());
bp::def("computeJacobians",(const Data::Matrix6x &(*)(const Model &, Data &))&computeJacobians,
bp::def("computeJacobians",(const Data::Matrix6x &(*)(const Model &, Data &))&computeJointJacobians,
bp::args("Model","Data"),
"Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.\n"
"The result is accessible through data.J. This function assumes that forwardKinematics has been called before",
......@@ -96,7 +95,7 @@ namespace se3
"Computes the jacobian of a given given joint according to the given input configuration."
"If local is set to true, it returns the jacobian associated to the joint frame. Otherwise, it returns the jacobian of the frame coinciding with the world frame.");
bp::def("getJacobian",get_jacobian_proxy,
bp::def("getJointJacobian",get_jacobian_proxy,
bp::args("Model, the model of the kinematic tree",
"Data, the data associated to the model where the results are stored",
"Joint ID, the index of the joint.",
......@@ -104,20 +103,21 @@ namespace se3
"Computes the jacobian of a given given joint according to the given entries in data."
"If local is set to true, it returns the jacobian associated to the joint frame. Otherwise, it returns the jacobian of the frame coinciding with the world frame.");
bp::def("computeJacobiansTimeVariation",computeJacobiansTimeVariation,
bp::def("computeJacobiansTimeVariation",computeJointJacobiansTimeVariation,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)"),
"Calling computeJacobiansTimeVariation",
"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.",
bp::return_value_policy<bp::return_by_value>());
bp::def("getJacobianTimeVariation",get_jacobian_time_variation_proxy,
bp::def("getJointJacobianTimeVariation",get_jacobian_time_variation_proxy,
bp::args("Model, the model of the kinematic tree",
"Data, the data associated to the model where the results are stored",
"Joint ID, the index of the joint.",
"frame (true = local, false = world)"),
"Computes the Jacobian time variation of a specific joint frame expressed either in the world frame or in the local frame of the joint."
"You have to run computeJacobiansTimeVariation first."
"You have to call computeJointJacobiansTimeVariation first."
"If local is set to true, it returns the jacobian time variation associated to the joint frame. Otherwise, it returns the jacobian time variation of the frame coinciding with the world frame.");
}
......
......@@ -114,7 +114,7 @@ namespace se3
///
/// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
/// The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa se3::computeJacobians).
/// The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa se3::computeJointJacobians).
/// And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the world frame).
///
/// \param[in] model The model structure of the rigid body system.
......
......@@ -33,7 +33,7 @@ namespace se3
/// - se3::forwardKinematics
/// - se3::crba
/// - se3::nonLinearEffects
/// - se3::computeJacobians
/// - se3::computeJointJacobians
/// - se3::centerOfMass
/// - se3::jacobianCenterOfMass
/// - se3::kineticEnergy
......
......@@ -50,11 +50,11 @@ namespace se3
/**
* @brief Returns the jacobian of the frame expresssed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
* depending on the value of the template parameter rf.
* You must first call se3::computeJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
* You must first call se3::computeJointJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
*
* @tparam rf Reference frame in which the columns of the Jacobian are expressed.
* @remark Similarly to se3::getJacobian with LOCAL or WORLD templated parameters, if rf == LOCAL, this function returns the Jacobian of the frame expressed
* @remark Similarly to se3::getJointJacobian with LOCAL or WORLD templated parameters, if rf == LOCAL, this function returns the Jacobian of the frame expressed
* in the local coordinates of the frame, or if rl == WORDL, it returns the Jacobian expressed of the point coincident with the origin
* and expressed in a coordinate system aligned with the WORLD.
*
......@@ -63,7 +63,7 @@ namespace se3
* @param[in] frame_id Id of the operational Frame
* @param[out] J The Jacobian of the Frame expressed in the coordinates Frame.
*
* @warning The function se3::computeJacobians and se3::framesForwardKinematics should have been called first.
* @warning The function se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
*/
template<ReferenceFrame rf>
void getFrameJacobian(const Model & model,
......@@ -73,14 +73,14 @@ namespace se3
/**
* @brief Returns the jacobian of the frame expresssed the LOCAL frame coordinate system.
* You must first call se3::computeJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
* You must first call se3::computeJointJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[out] J The Jacobian of the Frame expressed in the coordinates Frame.
*
* @warning The function se3::computeJacobians and se3::framesForwardKinematics should have been called first.
* @warning The function se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
*/
inline void getFrameJacobian(const Model & model,
......
......@@ -68,7 +68,7 @@ namespace se3
const Model::JointIndex & joint_id = frame.parent;
if (rf == WORLD)
{
getJacobian<WORLD>(model,data,joint_id,J);
getJointJacobian<WORLD>(model,data,joint_id,J);
return;
}
......
......@@ -28,7 +28,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. This function computes also the forwardKinematics of the model.
///
/// \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. \sa se3::getJacobian for doing this specific extraction.
/// \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. \sa se3::getJointJacobian for doing this specific extraction.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
......@@ -37,29 +37,89 @@ namespace se3
/// \return The full model Jacobian (matrix 6 x model.nv).
///
inline const Data::Matrix6x &
computeJointJacobians(const Model & model,
Data & data,
const Eigen::VectorXd & q);
///
/// \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. This function computes also the forwardKinematics of the model.
///
/// \deprecated This function is now deprecated. Please refer now to se3::computeJointJacobians for similar function with updated name.
///
/// \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. \sa se3::getJointJacobian for doing this specific extraction.
///
/// \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).
///
/// \return The full model Jacobian (matrix 6 x model.nv).
///
PINOCCHIO_DEPRECATED
inline const Data::Matrix6x &
computeJacobians(const Model & model,
Data & data,
const Eigen::VectorXd & q);
const Eigen::VectorXd & q)
{ return computeJointJacobians(model,data,q); }
///
/// \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. This function assumes that se3::forwardKinematics has been called before.
///
/// \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. \sa se3::getJointJacobian for doing this specific extraction.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
///
/// \return The full model Jacobian (matrix 6 x model.nv).
///
inline const Data::Matrix6x &
computeJointJacobians(const Model & model,
Data & data);
///
/// \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. This function assumes that se3::forwardKinematics has been called before.
///
/// \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. \sa se3::getJacobian for doing this specific extraction.
/// \deprecated This function is now deprecated. Please refer now to se3::computeJointJacobians for similar function with updated name.
///
/// \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. \sa se3::getJointJacobian for doing this specific extraction.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
///
/// \return The full model Jacobian (matrix 6 x model.nv).
///
PINOCCHIO_DEPRECATED
inline const Data::Matrix6x &
computeJacobians(const Model & model,
Data & data);
Data & data)
{ return computeJointJacobians(model,data); }
///
/// \brief Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or in the local frame (rf = LOCAL) of the joint.
/// \note This jacobian is extracted from data.J. You have to run se3::computeJointJacobians before calling it.
///
/// \tparam rf Reference frame in which the Jacobian is expressed.
///
/// \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] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).
///
template<ReferenceFrame rf>
void getJointJacobian(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & J);
///
/// \brief Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or in the local frame (rf = LOCAL) of the joint.
/// \note This jacobian is extracted from data.J. You have to run se3::computeJacobians before calling it.
///
/// \deprecated This function is now deprecated. Please refer now to se3::getJointJacobian for similar function with updated name.
///
/// \tparam rf Reference frame in which the Jacobian is expressed.
///
/// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system.
......@@ -69,10 +129,12 @@ namespace se3
/// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).
///
template<ReferenceFrame rf>
PINOCCHIO_DEPRECATED
void getJacobian(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & J);
Data::Matrix6x & J)
{ getJointJacobian<rf>(model,data,jointId,J); }
///
/// \brief Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store the result in the input argument J.
......@@ -85,15 +147,40 @@ namespace se3
///
/// \return The Jacobian of the specific joint frame expressed in the local frame of the joint (matrix 6 x model.nv).
///
/// \remark This function is equivalent to call first computeJacobians(model,data,q) and then call getJacobian<true>(model,data,jointId,J).
/// \remark This function is equivalent to call first computeJacobians(model,data,q) and then call getJointJacobian<LOCAL>(model,data,jointId,J).
/// It is worth to call jacobian if you only need a single Jacobian for a specific joint. Otherwise, for several Jacobians, it is better
/// to call computeJacobians(model,data,q) followed by getJacobian<true>(model,data,jointId,J) for each Jacobian.
/// to call computeJacobians(model,data,q) followed by getJointJacobian<LOCAL>(model,data,jointId,J) for each Jacobian.
///
inline void jointJacobian(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Model::JointIndex jointId,
Data::Matrix6x & J);
///
/// \brief Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store the result in the input argument J.
///
/// \deprecated This function is now deprecated. Please refer now to se3::jointJacobian for similar function with updated name.
///
/// \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] jointId The id of the joint refering to model.joints[jointId].
/// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero().
///
/// \return The Jacobian of the specific joint frame expressed in the local frame of the joint (matrix 6 x model.nv).
///
/// \remark This function is equivalent to call first computeJacobians(model,data,q) and then call getJointJacobian<LOCAL>(model,data,jointId,J).
/// It is worth to call jacobian if you only need a single Jacobian for a specific joint. Otherwise, for several Jacobians, it is better
/// to call computeJacobians(model,data,q) followed by getJointJacobian<LOCAL>(model,data,jointId,J) for each Jacobian.
///
PINOCCHIO_DEPRECATED
inline void jacobian(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Model::JointIndex jointId,
Data::Matrix6x & J);
Data::Matrix6x & J)
{ jointJacobian(model,data,q,jointId,J); }
///
/// \brief Computes the Jacobian of a specific joint frame expressed in the local frame of the joint. The result is stored in data.J.
......@@ -107,19 +194,19 @@ namespace se3
///
PINOCCHIO_DEPRECATED
inline const Data::Matrix6x &
jacobian(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Model::JointIndex jointId)
jointJacobian(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Model::JointIndex jointId)
{
data.J.setZero();
jacobian(model,data,q,jointId,data.J);
return data.J;
}
///
/// \brief Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depends both on q and v
/// \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.
///
///
......@@ -131,15 +218,56 @@ namespace se3
/// \return The full model Jacobian (matrix 6 x model.nv).
///
inline const Data::Matrix6x &
computeJointJacobiansTimeVariation(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
///
/// \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.
///
/// \deprecated This function is now deprecated. Please refer now to se3::computeJointJacobiansTimeVariation for similar function with updated name.
///
/// \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).
///
PINOCCHIO_DEPRECATED
inline const Data::Matrix6x &
computeJacobiansTimeVariation(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
const Eigen::VectorXd & v)
{ return computeJointJacobiansTimeVariation(model,data,q,v); }
///
/// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD) or in the local frame (rf = LOCAL) of the joint.
/// \note This jacobian is extracted from data.dJ. You have to run se3::computeJacobiansTimeVariation before calling it.
///
/// \deprecated This function is now deprecated. Please refer now to se3::getJointJacobianTimeVariation for similar function with updated name.
///
/// \tparam rf Reference frame in which the Jacobian is expressed.
///
/// \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<ReferenceFrame rf>
void getJointJacobianTimeVariation(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & dJ);
///
/// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD) or in the local frame (rf = LOCAL) of the joint.
/// \note This jacobian is extracted from data.dJ. You have to run se3::computeJointJacobiansTimeVariation before calling it.
///
/// \tparam rf Reference frame in which the Jacobian is expressed.
///
/// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system.
......@@ -149,10 +277,12 @@ namespace se3
/// \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<ReferenceFrame rf>
PINOCCHIO_DEPRECATED
void getJacobianTimeVariation(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & dJ);
Data::Matrix6x & dJ)
{ getJointJacobianTimeVariation<rf>(model,data,jointId,dJ); }
} // namespace se3
......
......@@ -25,14 +25,14 @@
namespace se3
{
struct JacobiansForwardStep : public fusion::JointVisitor<JacobiansForwardStep>
struct JointJacobiansForwardStep : public fusion::JointVisitor<JointJacobiansForwardStep>
{
typedef boost::fusion::vector <const se3::Model &,
se3::Data &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(JacobiansForwardStep);
JOINT_VISITOR_INIT(JointJacobiansForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
......@@ -56,25 +56,25 @@ namespace se3
};
inline const Data::Matrix6x &
computeJacobians(const Model & model, Data & data,
const Eigen::VectorXd & q)
computeJointJacobians(const Model & model, Data & data,
const Eigen::VectorXd & q)
{
assert(model.check(data) && "data is not consistent with model.");
for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i )
{
JacobiansForwardStep::run(model.joints[i],data.joints[i],
JacobiansForwardStep::ArgsType(model,data,q));
JointJacobiansForwardStep::run(model.joints[i],data.joints[i],
JointJacobiansForwardStep::ArgsType(model,data,q));
}
return data.J;
}
struct JacobiansForwardStep2 : public fusion::JointVisitor<JacobiansForwardStep2>
struct JointJacobiansForwardStep2 : public fusion::JointVisitor<JointJacobiansForwardStep2>
{
typedef boost::fusion::vector<se3::Data &> ArgsType;
JOINT_VISITOR_INIT(JacobiansForwardStep2);
JOINT_VISITOR_INIT(JointJacobiansForwardStep2);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
......@@ -89,14 +89,14 @@ namespace se3
};
inline const Data::Matrix6x &
computeJacobians(const Model & model, Data & data)
computeJointJacobians(const Model & model, Data & data)
{
assert(model.check(data) && "data is not consistent with model.");
for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i )
{
JacobiansForwardStep2::run(model.joints[i],data.joints[i],
JacobiansForwardStep2::ArgsType(data));
JointJacobiansForwardStep2::run(model.joints[i],data.joints[i],
JointJacobiansForwardStep2::ArgsType(data));
}
return data.J;
......@@ -106,7 +106,7 @@ namespace se3
world frame or in the local frame depending on the template argument. The
function computeJacobians should have been called first. */
template<ReferenceFrame rf>
void getJacobian(const Model & model,
void getJointJacobian(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & J)
......@@ -125,7 +125,7 @@ namespace se3
}
struct JacobianForwardStep : public fusion::JointVisitor<JacobianForwardStep>
struct JointJacobianForwardStep : public fusion::JointVisitor<JointJacobianForwardStep>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
......@@ -133,7 +133,7 @@ namespace se3
se3::Data::Matrix6x &
> ArgsType;
JOINT_VISITOR_INIT(JacobianForwardStep);
JOINT_VISITOR_INIT(JointJacobianForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
......@@ -156,22 +156,22 @@ namespace se3
};
inline void jacobian(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Model::JointIndex jointId,
Data::Matrix6x & J)
inline void jointJacobian(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Model::JointIndex jointId,
Data::Matrix6x & J)
{
assert(model.check(data) && "data is not consistent with model.");
data.iMf[jointId].setIdentity();
for( Model::JointIndex i=jointId;i>0;i=model.parents[i] )
{
JacobianForwardStep::run(model.joints[i],data.joints[i],
JacobianForwardStep::ArgsType(model,data,q,J));
JointJacobianForwardStep::run(model.joints[i],data.joints[i],
JointJacobianForwardStep::ArgsType(model,data,q,J));
}
}
struct JacobiansTimeVariationForwardStep : public fusion::JointVisitor<JacobiansTimeVariationForwardStep>
struct JointJacobiansTimeVariationForwardStep : public fusion::JointVisitor<JointJacobiansTimeVariationForwardStep>
{
typedef boost::fusion::vector <const se3::Model &,
se3::Data &,
......@@ -179,7 +179,7 @@ namespace se3
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT(JacobiansTimeVariationForwardStep);
JOINT_VISITOR_INIT(JointJacobiansTimeVariationForwardStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
......@@ -225,27 +225,27 @@ namespace se3
};
inline const Data::Matrix6x &
computeJacobiansTimeVariation(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
computeJointJacobiansTimeVariation(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));
JointJacobiansTimeVariationForwardStep::run(model.joints[i],data.joints[i],
JointJacobiansTimeVariationForwardStep::ArgsType(model,data,q,v));
}
return data.dJ;
}
template<ReferenceFrame rf>
void getJacobianTimeVariation(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & dJ)
void getJointJacobianTimeVariation(const Model & model,
const Data & data,