Commit 9a3879c0 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add additional signature for computeJacobians

parent 00698785
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -73,10 +73,17 @@ namespace se3
void exposeJacobian()
{
bp::def("computeJacobians",computeJacobians,
bp::def("computeJacobians",(const Data::Matrix6x &(*)(const Model &, Data &, const Eigen::VectorXd &))&computeJacobians,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)"),
"Calling computeJacobians",
"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::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",
bp::return_value_policy<bp::return_by_value>());
bp::def("jacobian",jacobian_proxy,
......
......@@ -41,6 +41,22 @@ namespace se3
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 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.
///
/// \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).
///
inline const Data::Matrix6x &
computeJacobians(const Model & model,
Data & 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::computeJacobians before calling it.
......
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -55,7 +55,6 @@ namespace se3
};
inline const Data::Matrix6x &
computeJacobians(const Model & model, Data & data,
const Eigen::VectorXd & q)
......@@ -71,6 +70,38 @@ namespace se3
return data.J;
}
struct JacobiansForwardStep2 : public fusion::JointVisitor<JacobiansForwardStep2>
{
typedef boost::fusion::vector<se3::Data &> ArgsType;
JOINT_VISITOR_INIT(JacobiansForwardStep2);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
se3::Data & data)
{
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
}
};
inline const Data::Matrix6x &
computeJacobians(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));
}
return data.J;
}
/* Return the jacobian of the output frame attached to joint <jointId> in the
world frame or in the local frame depending on the template argument. The
function computeJacobians should have been called first. */
......
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -71,6 +71,13 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
jacobian(model,data,q,idx,XJrh);
BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
/* Test computeJacobians with pre-computation of the forward kinematics */
Data data_fk(model);
forwardKinematics(model, data_fk, q);
computeJacobians(model, data_fk);
BOOST_CHECK(data_fk.J.isApprox(data.J));
}
......
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