Commit 4f73226c authored by jcarpent's avatar jcarpent Committed by Justin Carpentier
Browse files

[Algo] Remove overloading of getFrameJacobians

It now only returns the Jacobian of the FRAME expressed in the local
coordinate frame of the FRAME itself.
parent dc5b4253
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -25,26 +25,25 @@ namespace se3
static Data::Matrix6x frame_jacobian_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Model::FrameIndex frame_id,
bool local,
bool update_geometry
)
const Model::FrameIndex frame_id)
{
Data::Matrix6x J(6,model.nv); J.setZero();
if (update_geometry)
{
computeJacobians(model,data,q);
framesForwardKinematics(model,data);
}
if(local) getFrameJacobian<true> (model, data, frame_id, J);
else getFrameJacobian<false> (model, data, frame_id, J);
getFrameJacobian(model, data, frame_id, J);
return J;
}
static Data::Matrix6x frame_jacobian_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id,
const Eigen::VectorXd & q)
{
computeJacobians(model,data,q);
framesForwardKinematics(model,data);
return frame_jacobian_proxy(model, data, frame_id);
}
void exposeFramesAlgo()
{
bp::def("framesKinematics",
......@@ -61,16 +60,25 @@ namespace se3
"And computes the placements of all the operational frames"
"and put the results in data.");
bp::def("frameJacobian",frame_jacobian_proxy,
bp::def("frameJacobian",
(Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex, const Eigen::VectorXd &))&frame_jacobian_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Operational frame ID (int)",
"frame (true = local, false = world)",
"update_geometry (true = recompute the kinematics)"),
"Call computeJacobians if update_geometry is true. If not, user should call computeJacobians first."
"Then call getJacobian and return the resulted jacobian matrix. Attention: if update_geometry is true, the "
"function computes all the jacobians of the model. It is therefore outrageously costly wrt a dedicated "
"call. Use only with update_geometry for prototyping.");
"Configuration q (size Model::nq)"),
"Compute the Jacobian of the frame given by its ID."
"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,
bp::args("Model","Data",
"Operational frame ID (int)"),
"Compute the Jacobian of the frame given by its ID."
"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.");
}
} // namespace python
......
......@@ -148,7 +148,6 @@ class RobotWrapper(object):
else:
se3.updateGeometryPlacements(self.model, self.data, self.collision_model, self.collision_data, q)
# --- ACCESS TO NAMES ----
# Return the index of the joint whose name is given in argument.
def index(self, name):
......
......@@ -53,22 +53,18 @@ namespace se3
);
/**
* @brief Returns the jacobian of the frame expresssed in the world frame or
in the local frame depending on the template argument.
* @brief Returns the jacobian of the frame expresssed in the local frame depending on the template argument.
* You must first call se3::framesForwardKinematics and se3::computeJacobians to update values in data structure.
* @remark Expressed in the local frame, the jacobian maps the joint velocity vector to the spatial velocity of the center of the frame, expressed in the frame coordinates system. Expressed in the global frame, the jacobian maps to the spatial velocity of the point coinciding with the center of the world and attached to the frame.
* @remark Expressed in the local frame, the jacobian maps the joint velocity vector to the spatial velocity of the center of the frame, expressed in the frame coordinates system.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational frame we want to compute the jacobian
* @param[out] J The Jacobian of the
* @param[in] frame_id Id of the operational Frame
* @param[out] J The Jacobian of the Frame expressed in the coordinates Frame.
*
* @tparam local_frame If true, the jacobian is expressed in the local frame. Otherwise, the jacobian is expressed in the world frame.
*
* @warning The function se3::computeJacobians should have been called first
* @warning The function se3::computeJacobians and se3::framesForwardKinematics should have been called first.
*/
template<bool local_frame>
inline void getFrameJacobian(const Model & model,
const Data& data,
const Model::FrameIndex frame_id,
......@@ -112,9 +108,6 @@ namespace se3
framesForwardKinematics(model, data);
}
template<bool local_frame>
inline void getFrameJacobian(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
......@@ -124,24 +117,15 @@ namespace se3
assert(data.J.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
const Model::JointIndex & parent = model.frames[frame_id].parent;
const SE3 & oMframe = data.oMf[frame_id];
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;
if(!local_frame)
getJacobian<local_frame>(model, data, parent, J);
// Lever between the joint center and the frame center expressed in the global frame
const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
if(!local_frame)
J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
}
......
......@@ -66,50 +66,27 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
se3::Data data(model);
se3::Data data_ref(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd q_dot = VectorXd::Ones(model.nv);
/// In global frame
Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
/// In local frame
Model::Index idx = model.getFrameId(frame_name);
const Frame & frame = model.frames[idx];
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
computeJacobians(model,data,q);
computeJacobians(model,data_ref,q);
framesForwardKinematics(model,data);
getFrameJacobian<false>(model,data,idx,Jof);
getJacobian<false>(model, data_ref, parent_idx, Joj);
Motion nu_frame(Jof*q_dot);
Motion nu_joint(Joj*q_dot);
SE3 translation(SE3::Identity());
translation.translation(data.oMi[parent_idx].rotation()*frame.placement.translation());
Motion nu_frame_from_nu_joint(translation.actInv(nu_joint));
BOOST_CHECK(nu_frame.isApprox(nu_frame_from_nu_joint, 1e-12));
/// In local frame
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian<true>(model,data,idx,Jff);
getFrameJacobian(model,data,idx,Jff);
getJacobian<true>(model, data_ref, parent_idx, Jjj);
nu_frame = Motion(Jff*q_dot);
nu_joint = Motion(Jjj*q_dot);
Motion nu_frame = Motion(Jff*q_dot);
Motion nu_joint = Motion(Jjj*q_dot);
const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix();
Data::Matrix6x Jjj_from_frame(jXf * Jff);
BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
nu_frame_from_nu_joint = frame.placement.act(nu_frame);
BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
}
......
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