Unverified Commit 92ffa68b authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #526 from stack-of-tasks/devel

Merge devel into master
parents a545c921 6be296ec
Pipeline #1526 passed with stage
in 376 minutes and 42 seconds
# Please don't edit this file, and use the version generated at
# http://rainboard.laas.fr/project/pinocchio/.gitlab-ci.yml
variables:
GIT_SUBMODULE_STRATEGY: "recursive"
GIT_DEPTH: "3"
CCACHE_BASEDIR: "${CI_PROJECT_DIR}"
CCACHE_DIR: "${CI_PROJECT_DIR}/ccache"
......@@ -13,12 +15,12 @@ cache:
- gh-pages
script:
- mkdir -p ccache
- cd /root/robotpkg/math/pinocchio
- cd /root/robotpkg/math
- git pull
- cd pinocchio
- make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
- make install
- cd work.$(hostname)/$(make show-var VARNAME=DISTNAME)
- make build_tests
- make test
robotpkg-pinocchio-14.04-release:
......@@ -38,35 +40,43 @@ robotpkg-pinocchio-18.04-release:
- gh-pages
script:
- mkdir -p ccache
- cd /root/robotpkg/math/py-pinocchio
- cd /root/robotpkg/math
- git pull
- cd pinocchio
- make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
- cd ..
- cd py-pinocchio
- make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
- make install
- cd work.$(hostname)/$(make show-var VARNAME=DISTNAME)
- make test
robotpkg-py-pinocchio-py3-18.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:18.04
robotpkg-py-pinocchio-16.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:16.04
robotpkg-py-pinocchio-18.04-release:
robotpkg-py-pinocchio-14.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:18.04
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:14.04
robotpkg-py-pinocchio-py3-14.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:14.04
allow_failure: true
robotpkg-py-pinocchio-14.04-release:
robotpkg-py-pinocchio-16.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:14.04
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:16.04
robotpkg-py-pinocchio-py3-16.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:16.04
allow_failure: true
robotpkg-py-pinocchio-18.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:18.04
robotpkg-py-pinocchio-py3-18.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:18.04
allow_failure: true
doc-coverage:
<<: *robotpkg-py-pinocchio
......@@ -86,4 +96,3 @@ doc-coverage:
paths:
- doxygen-html/
- coverage/
......@@ -6,10 +6,10 @@ Pinocchio: a C++ library for efficient Rigid Multi-body Dynamics computations
[![Coverage Status](https://coveralls.io/repos/github/stack-of-tasks/pinocchio/badge.svg?branch=devel)](https://coveralls.io/github/stack-of-tasks/pinocchio?branch=devel)
[![Coverity Scan Build Status](https://scan.coverity.com/projects/7824/badge.svg)](https://scan.coverity.com/projects/pinocchio)
**Pinocchio** instatiates state-of-the-art Rigid Body Algotithms for poly-articulated systems based on revisited Roy Featherstone's algorithms.
**Pinocchio** instantiates state-of-the-art Rigid Body Algorithms for poly-articulated systems based on revisited Roy Featherstone's algorithms.
In addition, **Pinocchio** instantiates analytical derivatives of the main Rigid-Body Algorithms like the Recursive Newton-Euler Algorithms or the Articulated-Body Algorithm.
**Pinocchio** is first tailored for legged robotics applications, but it can be used in extra contextes.
It is built upon Eigen for linear algebra and FCL for collision detections. **Pinocchio** comes with a Python interface for fast code protyping.
**Pinocchio** is first tailored for legged robotics applications, but it can be used in extra contexts.
It is built upon Eigen for linear algebra and FCL for collision detection. **Pinocchio** comes with a Python interface for fast code prototyping.
**Pinocchio** is now at the hearth of various robotics softwares as the [Stack-of-Tasks](http://stack-of-tasks.github.io) or the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc).
......@@ -27,7 +27,7 @@ We provide some basic examples on how to use **Pinocchio** in Python in the [exa
## Tutorials
**Pinocchio** is comming with a large bunch of tutorials aiming at introducting the basic tools for robotics control.
**Pinocchio** is coming with a large bunch of tutorials aiming at introducing the basic tools for robotics control.
The content of the tutorials are described [here](http://projects.laas.fr/gepetto/index.php/Teach/Supaero2018) and the source code of these tutorials is located [here](https://github.com/stack-of-tasks/pinocchio-tutorials).
## Dependencies
......@@ -47,7 +47,7 @@ The content of the tutorials are described [here](http://projects.laas.fr/gepett
### Optional dependencies
- urdfdom (version >= 0.2)
- LUA 5.1
- [FCL](https://github.com/flexible-collision-library/fcl)
- [HPP-FCL](https://github.com/humanoid-path-planner/hpp-fcl)
### Python bindings
- Python 2.7 or 3.0
......@@ -96,4 +96,4 @@ If you have taken part to the development of **Pinocchio**, feel free to add you
## Acknowledgments
The development of **Pinocchio** is supported by the [Gepetto team](http://projects.laas.fr/gepetto/) @LAAS-CNRS.
The development of **Pinocchio** is supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr) and the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr).
......@@ -45,20 +45,88 @@ namespace se3
)
{
computeJointJacobians(model,data,q);
framesForwardKinematics(model,data);
updateFramePlacements(model,data);
return get_frame_jacobian_proxy(model, data, frame_id, rf);
}
static Data::Matrix6x
get_frame_jacobian_time_variation_proxy(const Model & model,
Data & data,
Model::FrameIndex jointId,
ReferenceFrame rf)
{
Data::Matrix6x dJ(6,model.nv); dJ.setZero();
if(rf == LOCAL) getFrameJacobianTimeVariation<LOCAL>(model,data,jointId,dJ);
else getFrameJacobianTimeVariation<WORLD>(model,data,jointId,dJ);
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);
updateFramePlacements(model,data);
return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
}
static Motion get_frame_velocity_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id
)
{
Motion v;
getFrameVelocity(model,data,frame_id,v);
return v;
}
static Motion get_frame_acceleration_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id
)
{
Motion a;
getFrameAcceleration(model,data,frame_id,a);
return a;
}
void exposeFramesAlgo()
{
bp::def("framesKinematics",
(void (*)(const Model &, Data &))&framesForwardKinematics,
bp::def("updateFramePlacements",
(void (*)(const Model &, Data &))&updateFramePlacements,
bp::args("Model","Data"),
"Computes the placements of all the operational frames according to the current joint placement stored in data"
"and put the results in data.");
"and puts the results in data.");
bp::def("updateFramePlacement",
(const SE3 & (*)(const Model &, Data &, const Model::FrameIndex))&updateFramePlacement,
bp::args("Model","Data","Operational frame ID (int)"),
"Computes the placement of the given operational frames according to the current joint placement stored in data,"
"puts the results in data and returns it.",
bp::return_value_policy<bp::return_by_value>());
bp::def("getFrameVelocity",
(Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_velocity_proxy,
bp::args("Model","Data","Operational frame ID (int)"),
"Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
"Fist or second order forwardKinematics should be called first.");
bp::def("getFrameAcceleration",
(Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_acceleration_proxy,
bp::args("Model","Data","Operational frame ID (int)"),
"Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
"Second order forwardKinematics should be called first.");
bp::def("framesKinematics",
bp::def("framesForwardKinematics",
(void (*)(const Model &, Data &, const Eigen::VectorXd &))&framesForwardKinematics,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
......@@ -87,7 +155,28 @@ namespace se3
"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 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",
"Frame ID, the index of the frame.",
"Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
"Returns the Jacobian time variation of a specific frame (specified by Frame ID) expressed either in the world or the local frame."
"You have to call computeJointJacobiansTimeVariation and framesKinematics first."
"If rf is set to LOCAL, it returns the jacobian time variation associated to the frame index. Otherwise, it returns the jacobian time variation of the frame coinciding with the world frame.");
}
} // namespace python
} // namespace se3
......@@ -78,7 +78,7 @@ namespace se3
"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("computeJointJacobians",(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"
......@@ -94,7 +94,7 @@ namespace se3
"update_kinematics (true = update the value of the total jacobian)"),
"Computes the jacobian of a given given joint according to the given input configuration."
"If rf is set to LOCAL, it returns the jacobian associated to the joint frame. Otherwise, it returns the jacobian of the frame coinciding with the world frame.");
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",
......@@ -116,7 +116,7 @@ namespace se3
"Data, the data associated to the model where the results are stored",
"Joint ID, the index of the joint.",
"Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.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."
"Computes the Jacobian time variation of a specific joint expressed either in the world frame or in the local frame of the joint."
"You have to call computeJointJacobiansTimeVariation first."
"If rf is set to LOCAL, 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.");
}
......
......@@ -20,17 +20,23 @@
#include <iostream>
#include <Python.h>
#include <boost/shared_ptr.hpp>
#include <boost/version.hpp>
// Boost 1.58
#if BOOST_VERSION / 100 % 1000 == 58
#include <fstream>
#endif
namespace se3
{
namespace python
{
namespace bp = boost::python;
Model buildModel(const std::string & filename, const std::string & model_name, bool verbose) throw (bp::error_already_set)
{
Py_Initialize();
bp::object main_module = bp::import("__main__");
// Get a dict for the global namespace to exec further python code with
bp::dict globals = bp::extract<bp::dict>(main_module.attr("__dict__"));
......@@ -42,13 +48,23 @@ namespace se3
// can update as you want.
try
{
// Boost 1.58
#if BOOST_VERSION / 100 % 1000 == 58
// Avoid a segv with exec_file
// See: https://github.com/boostorg/python/pull/15
std::ifstream t(filename.c_str());
std::stringstream buffer;
buffer << t.rdbuf();
bp::exec(buffer.str().c_str(), globals);
#else // default implementation
bp::exec_file((bp::str)filename, globals);
#endif
}
catch (bp::error_already_set & e)
{
PyErr_PrintEx(0);
}
Model model;
try
{
......@@ -64,10 +80,10 @@ namespace se3
std::cout << "Your model has been built. It has " << model.nv;
std::cout << " degrees of freedom." << std::endl;
}
// close interpreter
Py_Finalize();
return model;
}
} // namespace python
......
......@@ -21,7 +21,15 @@ from __future__ import print_function
from . import libpinocchio_pywrap as se3
from .deprecation import deprecated
@deprecated("This function has been renamed computeJointJacobians and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJacobians.")
@deprecated("This function has been renamed updateFramePlacements when taking two arguments, and framesForwardKinematics when taking three. Please change your code to the appropriate method.")
def framesKinematics(model,data,q=None):
if q is None:
se3.updateFramePlacements(model,data)
else:
se3.framesForwardKinematics(model,data,q)
@deprecated("This function has been renamed computeJointJacobians and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJointJacobians.")
def computeJacobians(model,data,q=None):
if q is None:
return se3.computeJointJacobians(model,data)
......@@ -42,7 +50,7 @@ def getJacobian(model,data,jointId,local):
else:
return se3.getJointJacobian(model,data,jointId,se3.ReferenceFrame.WORLD)
@deprecated("This function has been renamed computeJacobiansTimeVariation and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJacobiansTimeVariation.")
@deprecated("This function has been renamed computeJacobiansTimeVariation and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJointJacobiansTimeVariation.")
def computeJacobiansTimeVariation(model,data,q,v):
return se3.computeJointJacobiansTimeVariation(model,data,q,v)
......
......@@ -30,6 +30,8 @@ def deprecated(instructions):
return func(*args, **kwargs)
if wrapper.__doc__ is None:
wrapper.__doc__ = instructions
return wrapper
return decorator
......@@ -78,6 +78,22 @@ class RobotWrapper(object):
return self.data.com[0], self.data.vcom[0], self.data.acom[0]
return se3.centerOfMass(self.model, self.data, q)
def vcom(self, q, v):
se3.centerOfMass(self.model, self.data, q, v)
return self.data.vcom[0]
def acom(self, q, v, a):
se3.centerOfMass(self.model, self.data, q, v, a)
return self.data.acom[0]
def centroidalMomentum(self, q, v):
se3.ccrba(self.model, self.data, q, v)
return self.data.hg
def centroidalMomentumVariation(self, q, v, a):
se3.dccrba(self.model, self.data, q, v)
return se3.Force(self.data.Ag*a+self.data.dAg*v)
def Jcom(self, q):
return se3.jacobianCenterOfMass(self.model, self.data, q)
......@@ -99,7 +115,11 @@ class RobotWrapper(object):
else:
se3.forwardKinematics(self.model, self.data, q)
@deprecated("This method is now renamed placement. Please use placement instead.")
def position(self, q, index, update_kinematics=True):
return self.placement(q, index, update_kinematics)
def placement(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q)
return self.data.oMi[index]
......@@ -114,32 +134,29 @@ class RobotWrapper(object):
se3.forwardKinematics(self.model, self.data, q, v, a)
return self.data.a[index]
@deprecated("This method is now renamed framePlacement. Please use framePlacement instead.")
def framePosition(self, q, index, update_kinematics=True):
return self.framePlacement(q, index, update_kinematics)
def framePlacement(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q)
frame = self.model.frames[index]
parentPos = self.data.oMi[frame.parent]
return parentPos.act(frame.placement)
return se3.updateFramePlacement(self.model, self.data, index)
def frameVelocity(self, q, v, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v)
frame = self.model.frames[index]
parentJointVel = self.data.v[frame.parent]
return frame.placement.actInv(parentJointVel)
return se3.getFrameVelocity(self.model, self.data, index)
def frameAcceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a)
frame = self.model.frames[index]
parentJointAcc = self.data.a[frame.parent]
return frame.placement.actInv(parentJointAcc)
return se3.getFrameAcceleration(self.model, self.data, index)
def frameClassicAcceleration(self, index):
f = self.model.frames[index]
a = f.placement.actInv(self.data.a[f.parent])
v = f.placement.actInv(self.data.v[f.parent])
a.linear += np.cross(v.angular.T, v.linear.T).T
v = se3.getFrameVelocity(self.model, self.data, index)
a = se3.getFrameAcceleration(self.model, self.data, index)
a.linear += np.cross(v.angular, v.linear, axis=0)
return a;
@deprecated("This method is now deprecated. Please use jointJacobian instead. It will be removed in release 1.4.0 of Pinocchio.")
......@@ -175,9 +192,12 @@ class RobotWrapper(object):
else:
se3.updateGeometryPlacements(self.model, self.data, geom_model, geom_data)
@deprecated("This method is now renamed framesForwardKinematics. Please use framesForwardKinematics instead.")
def framesKinematics(self, q):
se3.framesKinematics(self.model, self.data, q)
se3.framesForwardKinematics(self.model, self.data, q)
def framesForwardKinematics(self, q):
se3.framesForwardKinematics(self.model, self.data, q)
'''
It computes the Jacobian of frame given by its id (frame_id) either expressed in the
......@@ -188,7 +208,7 @@ class RobotWrapper(object):
'''
Similar to getFrameJacobian but it also calls before se3.computeJointJacobians and
se3.framesKinematics to update internal value of self.data related to frames.
se3.framesForwardKinematics to update internal value of self.data related to frames.
'''
def frameJacobian(self, q, frame_id, rf_frame):
return se3.frameJacobian(self.model, self.data, q, frame_id, rf_frame)
......
......@@ -2,11 +2,11 @@
This directory contains minimal examples on how to use **Pinocchio** with the Python bindings.
# Loading a model
## Loading a model
- Loading a URDF model: `python -i load-urdf.py`
# Computing analytical derivatives of rigid body dynamics algorithms
## Computing analytical derivatives of rigid body dynamics algorithms
- Computing forward kinematics derivatives: `python -i kinematics-derivatives.py`
- Computing forward dynamics (fd) derivatives: `python -i fd-derivatives.py`
......
......@@ -25,13 +25,42 @@ namespace se3
{
/**
* @brief Updates the position of each frame contained in the model
* @brief Updates the placement of each frame contained in the model
*
* @param[in] model The kinematic model.
* @param data Data associated to model.
*
* @warning One of the algorithms forwardKinematics should have been called first
*/
inline void updateFramePlacements(const Model & model,
Data & data);
/**
* @brief Updates the placement of the given frame.
*
* @param[in] model The kinematic model.
* @param data Data associated to model.
* @param[in] frame_id Id of the operational Frame.
*
* @return A reference to the frame placement stored in data.oMf[frame_id]
*
* @warning One of the algorithms forwardKinematics should have been called first
*/
inline const SE3 & updateFramePlacement(const Model & model,
Data & data,
const Model::FrameIndex frame_id);
/**
* @brief Updates the placement of each frame contained in the model
*
* @deprecated This function is now deprecated. Please call se3::updateFramePlacements for same functionality
*
* @param[in] model The kinematic model.
* @param data Data associated to model.
*
* @warning One of the algorithms forwardKinematics should have been called first
*/
PINOCCHIO_DEPRECATED
inline void framesForwardKinematics(const Model & model,
Data & data);
......@@ -48,14 +77,47 @@ namespace se3
const Eigen::VectorXd & q);
/**
* @brief Returns the jacobian of the frame expresssed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
* @brief Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
* You must first call se3::forwardKinematics to update placement and velocity 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] frame_v The spatial velocity of the Frame expressed in the coordinates Frame.
*
* @warning Fist or second order forwardKinematics should have been called first
*/
inline void getFrameVelocity(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_v);
/**
* @brief Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
* You must first call se3::forwardKinematics 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] frame_a The spatial acceleration of the Frame expressed in the coordinates Frame.
*
* @warning Second order forwardKinematics should have been called first
*/
inline void getFrameAcceleration(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Motion & frame_a);
/**
* @brief Returns the jacobian of the frame expressed either 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::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::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
* in the local coordinates of the frame, or if rl == WORLD, 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
......@@ -63,7 +125,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::computeJointJacobians and se3::framesForwardKinematics should have been called first.
* @warning The functions se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
*/
template<ReferenceFrame rf>
void getFrameJacobian(const Model & model,
......@@ -74,6 +136,7 @@ namespace se3
/**
* @brief Returns the jacobian of the frame expresssed the LOCAL frame coordinate system.
* You must first call se3::computeJointJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
\deprecated This function is now deprecated. Please call se3::getFrameJacobian<ReferenceFrame> for same functionality
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
......@@ -82,11 +145,30 @@ namespace se3
*
* @warning The function se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
*/
PINOCCHIO_DEPRECATED
inline void getFrameJacobian(const Model & model,
const Data & data,
const Model::FrameIndex frame_id,
Data::Matrix6x & J);