Commit f8c5cee4 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[bindings/framejacobiantimevariation] add a function...

[bindings/framejacobiantimevariation] add a function framesJacobianTimeVariation in the groove of framesJacobian that also calls compute and fk.
parent 54f2f96d
......@@ -65,6 +65,20 @@ namespace se3
return dJ;
}
static Data::Matrix6x frame_jacobian_time_variation_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Model::FrameIndex frame_id,
ReferenceFrame rf
)
{
computeJointJacobiansTimeVariation(model,data,q,v);
framesForwardKinematics(model,data);
return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
}
void exposeFramesAlgo()
{
......@@ -104,6 +118,18 @@ namespace se3
"where v is the time derivative of the configuration q.\n"
"Be aware that computeJointJacobians and framesKinematics must have been called first.");
bp::def("frameJacobianTimeVariation",
(Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &,const Eigen::VectorXd &, const Model::FrameIndex, ReferenceFrame))&frame_jacobian_time_variation_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
"Operational frame ID (int)",
"Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
"Computes the Jacobian Time Variation of the frame given by its ID either in the local or the world frames."
"The columns of the Jacobian time variation 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("getFrameJacobianTimeVariation",get_frame_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",
......
......@@ -103,7 +103,7 @@ namespace se3
assert( dJ.cols() == data.dJ.cols() );
assert(model.check(data) && "data is not consistent with model.");
const Frame & frame = model.frames[frame_id];
const Frame & frame = model.frames[frameId];
const Model::JointIndex & joint_id = frame.parent;
if (rf == WORLD)
{
......@@ -113,7 +113,7 @@ namespace se3
if (rf == LOCAL)
{
const SE3 & oMframe = data.oMf[frame_id];
const SE3 & oMframe = data.oMf[frameId];
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])
......
Markdown is supported
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