Skip to content
GitLab
Commit 245a3fb9 authored by jcarpent's avatar jcarpent
Browse files

[Frame] Ajust API according to the Joint Jacobian API

parent d714e42b
Hide whitespace changes
Inline Side-by-side
bindings/python/algorithm/expose-frames.cpp
View file @ 245a3fb9
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -25,10 +25,14 @@ namespace se3
static Data::Matrix6x frame_jacobian_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id)
const Model::FrameIndex frame_id,
const bool local)
{
Data::Matrix6x J(6,model.nv); J.setZero();
getFrameJacobian(model, data, frame_id, J);
if(local)
getFrameJacobian<LOCAL>(model, data, frame_id, J);
else
getFrameJacobian<WORLD>(model, data, frame_id, J);
return J;
}
......@@ -36,12 +40,13 @@ namespace se3
static Data::Matrix6x frame_jacobian_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id,
const bool local,
const Eigen::VectorXd & q)
{
computeJacobians(model,data,q);
framesForwardKinematics(model,data);
return frame_jacobian_proxy(model, data, frame_id);
return frame_jacobian_proxy(model, data, frame_id, local);
}
void exposeFramesAlgo()
......@@ -61,20 +66,22 @@ namespace se3
"and put the results in data.");
bp::def("frameJacobian",
(Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex, const Eigen::VectorXd &))&frame_jacobian_proxy,
(Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex, const bool, const Eigen::VectorXd &))&frame_jacobian_proxy,
bp::args("Model","Data",
"Operational frame ID (int)",
"Configuration q (size Model::nq)"),
"Compute the Jacobian of the frame given by its ID."
"Configuration q (size Model::nq)",
"frame (true = local, false = world)"),
"Compute the Jacobian of the frame given by its ID either in the local or the world frames."
"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.");
bp::def("frameJacobian",
(Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex))&frame_jacobian_proxy,
(Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex, const bool))&frame_jacobian_proxy,
bp::args("Model","Data",
"Operational frame ID (int)"),
"Compute the Jacobian of the frame given by its ID."
"Operational frame ID (int)",
"frame (true = local, false = world)"),
"Compute the Jacobian of the frame given by its ID either in the local or the world frames."
"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"
......
src/algorithm/frames.hpp
View file @ 245a3fb9
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -47,12 +47,15 @@ namespace se3
const Eigen::VectorXd & q);
/**
* @brief Returns the jacobian of the frame expresssed in the local frame depending on the template argument.
* You must first call se3::computeJacobians followad by se3::framesForwardKinematics to update placement values in data structure.
* @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.
*
* @tparam rf Reference frame in which the columns of the Jacobian are expressed.
* @remark Compared to se3::getJacobian with LOCAL or WORLD templated parameters, this function only returns the Jacobian of the frame expressed
* in the local coordinates of Frame. Indeed, evaluated at the origin of the world, this function would return the same Jacobian as
* se3::getJacobian<se3::WORLD>(model,data,frame.parent,J).
* @remark Similarly to se3::getJacobian 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.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
......@@ -61,8 +64,26 @@ namespace se3
*
* @warning The function se3::computeJacobians and se3::framesForwardKinematics should have been called first.
*/
template<ReferenceFrame rf>
void getFrameJacobian(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Data::Matrix6x & J);
/**
* @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.
*
* @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.
*/
inline void getFrameJacobian(const Model & model,
const Data& data,
const Data & data,
const Model::FrameIndex frame_id,
Data::Matrix6x & J);
......
src/algorithm/frames.hxx
View file @ 245a3fb9
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -54,6 +54,7 @@ namespace se3
framesForwardKinematics(model, data);
}
template<ReferenceFrame rf>
inline void getFrameJacobian(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
......@@ -64,16 +65,33 @@ namespace se3
assert(model.check(data) && "data is not consistent with model.");
const Frame & frame = model.frames[frame_id];
const Model::JointIndex & parent = frame.parent;
const SE3 & oMframe = data.oMf[frame_id];
const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
const Model::JointIndex & joint_id = frame.parent;
if (rf == WORLD)
{
getJacobian<WORLD>(model,data,joint_id,J);
return;
}
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
if (rf == LOCAL)
{
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
const SE3 & oMframe = data.oMf[frame_id];
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
return;
}
}
inline void getFrameJacobian(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Data::Matrix6x & J)
{
getFrameJacobian<LOCAL>(model,data,frame_id,J);
}
} // namespace se3
......
unittest/frames.cpp
View file @ 245a3fb9
......@@ -88,6 +88,11 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
// In world frame
getFrameJacobian<WORLD>(model,data,idx,Jff);
getJacobian<WORLD>(model, data_ref, parent_idx, Jjj);
BOOST_CHECK(Jff.isApprox(Jjj));
}
BOOST_AUTO_TEST_SUITE_END ()
......
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