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

Merge pull request #946 from jcarpent/devel

Remove deprecated functions and methods
parents d7ca7d5d 9575797e
......@@ -40,8 +40,6 @@ int main(int argc, const char ** argv)
if(argc>1) filename = argv[1];
if( filename == "HS")
pinocchio::buildModels::humanoidRandom(model,true);
else if( filename == "H2" )
pinocchio::buildModels::humanoid2d(model);
else
pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
std::cout << "nq = " << model.nq << std::endl;
......
......@@ -76,9 +76,9 @@ int main(int argc, const char ** argv)
timer.tic();
SMOOTH(NBT)
{
jointJacobian(model,data,qs[_smooth],JOINT_ID,J);
computeJointJacobian(model,data,qs[_smooth],JOINT_ID,J);
}
std::cout << "jointJacobian = \t\t"; timer.toc(std::cout,NBT);
std::cout << "computeJointJacobian = \t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
......
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2019 CNRS INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
......@@ -21,24 +21,22 @@ namespace pinocchio
return J;
}
static Data::Matrix6x frame_jacobian_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Model::FrameIndex frame_id
)
static Data::Matrix6x compute_frame_jacobian_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Model::FrameIndex frame_id)
{
Data::Matrix6x J(6,model.nv); J.setZero();
frameJacobian(model, data, q, frame_id, J);
computeFrameJacobian(model, data, q, frame_id, J);
return J;
}
static Data::Matrix6x
get_frame_jacobian_time_variation_proxy(const Model & model,
Data & data,
Model::FrameIndex jointId,
ReferenceFrame 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();
getFrameJacobianTimeVariation(model,data,jointId,rf,dJ);
......@@ -51,8 +49,7 @@ namespace pinocchio
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Model::FrameIndex frame_id,
ReferenceFrame rf
)
const ReferenceFrame rf)
{
computeJointJacobiansTimeVariation(model,data,q,v);
updateFramePlacements(model,data);
......@@ -96,8 +93,8 @@ namespace pinocchio
"And computes the placements of all the operational frames"
"and put the results in data.");
bp::def("frameJacobian",
&frame_jacobian_proxy,
bp::def("computeFrameJacobian",
&compute_frame_jacobian_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Operational frame ID (int)"),
......@@ -117,8 +114,7 @@ namespace pinocchio
"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::def("frameJacobianTimeVariation",&frame_jacobian_time_variation_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
......@@ -139,5 +135,7 @@ namespace pinocchio
"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 pinocchio
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2019 CNRS INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
......@@ -11,13 +11,13 @@ namespace pinocchio
{
static Data::Matrix6x
jacobian_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
Model::JointIndex jointId)
compute_jacobian_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
Model::JointIndex jointId)
{
Data::Matrix6x J(6,model.nv); J.setZero();
jointJacobian(model,data,q,jointId,J);
computeJointJacobian(model,data,q,jointId,J);
return J;
}
......@@ -65,7 +65,7 @@ namespace pinocchio
"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("jointJacobian",jacobian_proxy,
bp::def("computeJointJacobian",compute_jacobian_proxy,
bp::args("Model, the model of the kinematic tree",
"Data, the data associated to the model where the results are stored",
"Joint configuration q (size Model::nq)",
......
......@@ -11,181 +11,6 @@ import warnings as _warnings
from . import libpinocchio_pywrap as pin
from .deprecation import deprecated, DeprecatedWarning
# deprecated StdVect_ (since: 19/04/2019)
StdVect_Frame = deprecated("Please use StdVec_Frame")(pin.StdVec_Frame)
StdVect_GeometryObject = deprecated("Please use StdVec_GeometryObject")(pin.StdVec_GeometryObject)
if pin.WITH_FCL_SUPPORT():
StdVect_Contact = deprecated("Please use StdVec_Contact")(pin.StdVec_Contact)
StdVect_CollisionResult = deprecated("Please use StdVec_CollisionResult")(pin.StdVec_CollisionResult)
StdVect_DistanceResult = deprecated("Please use StdVec_DistanceResult")(pin.StdVec_DistanceResult)
StdVect_SE3 = deprecated("Please use StdVec_SE3")(pin.StdVec_SE3)
StdVect_Force = deprecated("Please use StdVec_Force")(pin.StdVec_Force)
StdVect_Motion = deprecated("Please use StdVec_Motion")(pin.StdVec_Motion)
@deprecated("This function has been renamed to impulseDynamics for consistency with the C++ interface. Please change for impulseDynamics.")
def impulseDynamics(model, data, q, v_before, J, r_coeff=0.0, update_kinematics=True):
return pin.impulseDynamics(model, data, q, v_before, J, r_coeff, update_kinematics)
@deprecated("This function has been deprecated. Please use buildSampleModelHumanoid or buildSampleModelHumanoidRandom instead.")
def buildSampleModelHumanoidSimple(usingFF=True):
return pin.buildSampleModelHumanoidRandom(usingFF)
@deprecated("Static method Model.BuildHumanoidSimple has been deprecated. Please use function buildSampleModelHumanoid or buildSampleModelHumanoidRandom instead.")
def _Model__BuildHumanoidSimple(usingFF=True):
return pin.buildSampleModelHumanoidRandom(usingFF)
pin.Model.BuildHumanoidSimple = staticmethod(_Model__BuildHumanoidSimple)
@deprecated("Static method Model.BuildEmptyModel has been deprecated. Please use the empty Model constructor instead.")
def _Model__BuildEmptyModel():
return pin.Model()
pin.Model.BuildEmptyModel = staticmethod(_Model__BuildEmptyModel)
@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:
pin.updateFramePlacements(model,data)
else:
pin.framesForwardKinematics(model,data,q)
@deprecated("This function has been renamed computeJointJacobians and will be removed in future releases of Pinocchio. Please change for new computeJointJacobians.")
def computeJacobians(model,data,q=None):
if q is None:
return pin.computeJointJacobians(model,data)
else:
return pin.computeJointJacobians(model,data,q)
# This function is only deprecated when using a specific signature. Therefore, it needs special care
# Marked as deprecated on 19 Feb 2019
def jointJacobian(model, data, q, jointId, *args):
if len(args)==2:
message = ("This function signature has been deprecated and will be removed in future releases of Pinocchio. "
"Please change for the new signature of jointJacobian or use computeJointJacobian + getJointJacobian.")
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
rf = args[0]
update_kinematics = args[1]
if update_kinematics:
pin.computeJointJacobians(model,data,q)
return pin.getJointJacobian(model,data,jointId,rf)
else:
return pin.jointJacobian(model,data,q,jointId)
jointJacobian.__doc__ = (
pin.jointJacobian.__doc__
+ '\n\njointJacobian( (Model)Model, (Data)Data, (object)Joint configuration q (size Model::nq), (Index)jointId, ReferenceFrame rf, (bool)updateKinematics) -> object :'
+ '\n This function signature has been deprecated and will be removed in future releases of Pinocchio.'
)
# This function is only deprecated when using a specific signature. Therefore, it needs special care
# Marked as deprecated on 19 Feb 2019
def frameJacobian(model, data, q, frameId, *args):
if len(args)==1:
message = ("This function signature has been deprecated and will be removed in future releases of Pinocchio. "
"Please change for the new signature of frameJacobian or use computeJointJacobian + updateFramePlacements + getFrameJacobian.")
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
rf = args[0]
pin.computeJointJacobians(model,data,q)
pin.updateFramePlacements(model,data)
return pin.getFrameJacobian(model, data, frameId, rf);
else:
return pin.frameJacobian(model,data,q,frameId)
frameJacobian.__doc__ = (
pin.frameJacobian.__doc__
+ '\n\nframeJacobian( (Model)Model, (Data)Data, (object)Joint configuration q (size Model::nq), (Index)frameId, ReferenceFrame rf) -> object :'
+ '\n This function signature has been deprecated and will be removed in future releases of Pinocchio.'
)
@deprecated("This function has been renamed jointJacobian and will be removed in future releases of Pinocchio. Please change for new jointJacobian function.")
def jacobian(model,data,q,jointId,local,update_kinematics):
rf = pin.ReferenceFrame.LOCAL if local else pin.ReferenceFrame.WORLD
if update_kinematics:
pin.computeJointJacobians(model,data,q)
return pin.getJointJacobian(model,data,jointId,rf)
@deprecated("This function has been renamed getJointJacobian and will be removed in future releases of Pinocchio. Please change for new getJointJacobian function.")
def getJacobian(model,data,jointId,local):
if local:
return pin.getJointJacobian(model,data,jointId,pin.ReferenceFrame.LOCAL)
else:
return pin.getJointJacobian(model,data,jointId,pin.ReferenceFrame.WORLD)
@deprecated("This function has been renamed computeJacobiansTimeVariation and will be removed in future releases of Pinocchio. Please change for new computeJointJacobiansTimeVariation.")
def computeJacobiansTimeVariation(model,data,q,v):
return pin.computeJointJacobiansTimeVariation(model,data,q,v)
@deprecated("This function has been renamed getJointJacobianTimeVariation and will be removed in future releases of Pinocchio. Please change for new getJointJacobianTimeVariation function.")
def getJacobianTimeVariation(model,data,jointId,local):
if local:
return pin.getJointJacobianTimeVariation(model,data,jointId,pin.ReferenceFrame.LOCAL)
else:
return pin.getJointJacobianTimeVariation(model,data,jointId,pin.ReferenceFrame.WORLD)
@deprecated("This function has been renamed difference and will be removed in future releases of Pinocchio. Please change for new difference function.")
def differentiate(model,q0,q1):
return pin.difference(model,q0,q1)
@deprecated("This function has been renamed loadReferenceConfigurations and will be removed in future releases of Pinocchio. Please change for new loadReferenceConfigurations function.")
def getNeutralConfigurationFromSrdf(model, filename, verbose):
pin.loadReferenceConfigurations(model,filename,verbose)
return model.referenceConfigurations["half_sitting"]
@deprecated("This function has been renamed loadReferenceConfigurations and will be removed in future releases of Pinocchio. Please change for new loadReferenceConfigurations function.")
def getNeutralConfiguration(model, filename, verbose):
pin.loadReferenceConfigurations(model,filename,verbose)
return model.referenceConfigurations["half_sitting"]
@deprecated("This function has been renamed difference and will be removed in future releases of Pinocchio. Please change for new loadRotorParameters function.")
def loadRotorParamsFromSrdf(model, filename, verbose):
return pin.loadRotorParams(model,filename,verbose)
@deprecated("This function has been renamed difference and will be removed in future releases of Pinocchio. Please change for new removeCollisionPairs function.")
def removeCollisionPairsFromSrdf(model, geomModel, filename, verbose):
return pin.removeCollisionPairs(model,geomModel,filename,verbose)
# This function is only deprecated when using a specific signature. Therefore, it needs special care
def jacobianCenterOfMass(model, data, *args):
if len(args)==3:
message = "This function signature has been deprecated and will be removed in future releases of Pinocchio. Please change for one of the new signatures of the jacobianCenterOfMass function."
_warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
q = args[0]
computeSubtreeComs = args[1]
updateKinematics = args[2]
if updateKinematics:
return pin.jacobianCenterOfMass(model,data,q,computeSubtreeComs)
else:
return pin.jacobianCenterOfMass(model,data,computeSubtreeComs)
else:
return pin.jacobianCenterOfMass(model,data,*args)
jacobianCenterOfMass.__doc__ = (
pin.jacobianCenterOfMass.__doc__
+ '\n\njacobianCenterOfMass( (Model)Model, (Data)Data, (object)Joint configuration q (size Model::nq), (bool)computeSubtreeComs, (bool)updateKinematics) -> object :'
+ '\n This function signature has been deprecated and will be removed in future releases of Pinocchio.'
)
@deprecated("This function will be removed in future releases of Pinocchio. You can use exp or exp6.")
def exp6FromMotion(motion):
return pin.exp6(motion)
@deprecated("This function will be removed in future releases of Pinocchio. You can build a Motion object from a 6D vector and use the standard exp function to recover the same behavior.")
def exp6FromVector(vector6):
v = pin.Motion(vector6)
return pin.exp6(v)
@deprecated("This function will be removed in future releases of Pinocchio. You can use log or log6.")
def log6FromSE3(transform):
return pin.log6(transform)
@deprecated("This function will be removed in future releases of Pinocchio. You can use log or log6.")
def log6FromMatrix(matrix4):
return pin.log6(matrix4)
@deprecated("This function has been renamed getJointJacobian and will be removed in future releases of Pinocchio. Please change for new getJointJacobian function.")
def getJacobian(model,data,jointId,local):
if local:
return pin.getJointJacobian(model,data,jointId,pin.ReferenceFrame.LOCAL)
else:
return pin.getJointJacobian(model,data,jointId,pin.ReferenceFrame.WORLD)
# This function is only deprecated when using a specific signature. Therefore, it needs special care
# Marked as deprecated on 16 Sept 2019
def impulseDynamics(model, data, *args):
......@@ -239,6 +64,14 @@ forwardDynamics.__doc__ = (
+ '\n This function signature has been deprecated and will be removed in future releases of Pinocchio.'
)
@deprecated("This function has been renamed computeJointJacobian and will be removed in future releases of Pinocchio. Please change for new computeJointJacobian.")
def jointJacobian(model, data, q, jointId):
return pin.computeJointJacobian(model,data,q,jointId)
@deprecated("This function has been renamed computeFrameJacobian and will be removed in future releases of Pinocchio. Please change for new computeFrameJacobian.")
def frameJacobian(model, data, q, frameId):
return pin.computeFrameJacobian(model,data,q,frameId)
def computeCentroidalDynamics(model, data, q, v, a = None):
if a is None:
message = ("This function signature has been renamed and will be removed in future releases of Pinocchio. "
......
#
# Copyright (c) 2015-2019 CNRS
# Copyright (c) 2015-2019 CNRS INRIA
#
from . import libpinocchio_pywrap as pin
......@@ -79,10 +79,6 @@ class RobotWrapper(object):
def nle(self, q, v):
return pin.nonLinearEffects(self.model, self.data, q, v)
@deprecated("This method is now renamed nle. Please use nle instead.")
def bias(self, q, v):
return pin.nonLinearEffects(self.model, self.data, q, v)
def gravity(self, q):
return pin.computeGeneralizedGravity(self.model, self.data, q)
......@@ -95,10 +91,6 @@ class RobotWrapper(object):
else:
pin.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:
pin.forwardKinematics(self.model, self.data, q)
......@@ -114,10 +106,6 @@ class RobotWrapper(object):
pin.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:
pin.forwardKinematics(self.model, self.data, q)
......@@ -139,27 +127,15 @@ class RobotWrapper(object):
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.")
def jacobian(self, q, index, update_kinematics=True, local_frame=True):
if local_frame:
return pin.jointJacobian(self.model, self.data, q, index, pin.ReferenceFrame.LOCAL, update_kinematics)
else:
return pin.jointJacobian(self.model, self.data, q, index, pin.ReferenceFrame.WORLD, update_kinematics)
def jointJacobian(self, q, index, *args):
if len(args)==0:
return pin.jointJacobian(self.model, self.data, q, index)
else: # use deprecated signature (19 Feb 2019)
update_kinematics = True if len(args)==1 else args[1]
rf = args[0]
return pin.jointJacobian(self.model, self.data, q, index, rf_frame, update_kinematics)
@deprecated("This method has been renamed computeJointJacobian. Please use computeJointJacobian instead of jointJacobian.")
def jointJacobian(self, q, index):
return pin.computeJointJacobian(self.model, self.data, q, index)
def computeJointJacobian(self, q, index):
return pin.computeJointJacobian(self.model, self.data, q, index)
def getJointJacobian(self, index, rf_frame=pin.ReferenceFrame.LOCAL):
return pin.getFrameJacobian(self.model, self.data, index, rf_frame)
@deprecated("This method is now deprecated. Please use computeJointJacobians instead. It will be removed in release 1.4.0 of Pinocchio.")
def computeJacobians(self, q):
return pin.computeJointJacobians(self.model, self.data, q)
return pin.getJointJacobian(self.model, self.data, index, rf_frame)
def computeJointJacobians(self, q):
return pin.computeJointJacobians(self.model, self.data, q)
......@@ -177,29 +153,30 @@ class RobotWrapper(object):
else:
pin.updateGeometryPlacements(self.model, self.data, geom_model, geom_data)
@deprecated("This method is now renamed framesForwardKinematics. Please use framesForwardKinematics instead.")
def framesKinematics(self, q):
pin.framesForwardKinematics(self.model, self.data, q)
def framesForwardKinematics(self, q):
pin.framesForwardKinematics(self.model, self.data, q)
'''
It computes the Jacobian of frame given by its id (frame_id) either expressed in the
local coordinate frame or in the world coordinate frame.
'''
def getFrameJacobian(self, frame_id, rf_frame=pin.ReferenceFrame.LOCAL):
"""
It computes the Jacobian of frame given by its id (frame_id) either expressed in the
local coordinate frame or in the world coordinate frame.
"""
return pin.getFrameJacobian(self.model, self.data, frame_id, rf_frame)
'''
Similar to getFrameJacobian but does not need pin.computeJointJacobians and
pin.updateFramePlacements to update internal value of self.data related to frames.
'''
def frameJacobian(self, q, frame_id, rf_frame=None):
if rf_frame: # use deprecated signature (19 Feb 2019)
return pin.frameJacobian(self.model, self.data, q, frame_id, rf_frame)
else: # use normal signature
return pin.frameJacobian(self.model, self.data, q, frame_id)
@deprecated("This method has been renamed computeFrameJacobian. Please use computeFrameJacobian instead of frameJacobian.")
def frameJacobian(self, q, frame_id):
"""
Similar to getFrameJacobian but does not need pin.computeJointJacobians and
pin.updateFramePlacements to update internal value of self.data related to frames.
"""
return pin.computeFrameJacobian(self.model, self.data, q, frame_id)
def computeFrameJacobian(self, q, frame_id):
"""
Similar to getFrameJacobian but does not need pin.computeJointJacobians and
pin.updateFramePlacements to update internal value of self.data related to frames.
"""
return pin.computeFrameJacobian(self.model, self.data, q, frame_id)
def rebuildData(self):
"""Re-build the data objects. Needed if the models were modified.
......
......@@ -38,7 +38,7 @@ int main(int /* argc */, char ** /* argv */)
std::cout << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" << std::endl;
break;
}
pinocchio::jointJacobian(model,data,q,JOINT_ID,J);
pinocchio::computeJointJacobian(model,data,q,JOINT_ID,J);
const Eigen::VectorXd v = - svd.compute(J.topRows<3>()).solve(err);
q = pinocchio::integrate(model,q,v*DT);
if(!(i%10)) std::cout << "error = " << err.transpose() << std::endl;
......
......@@ -157,33 +157,6 @@ namespace pinocchio
const bool computeSubtreeComs = true)
{ centerOfMass(model,data,2,computeSubtreeComs); }
///
/// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
/// The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa pinocchio::computeJointJacobians).
/// And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the world frame).
///
/// \deprecated This function signature is now deprecated. The argument updateKinematics was redundant with the input argument q.
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorType Type of the joint configuration vector.
///
/// \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).
/// \param[in] computeSubtreeComs If true, the algorithm also computes the center of mass of the subtrees, expressed in the world coordinate frame.
/// \param[in] updateKinematics If true, the algorithm updates first the geometry of the tree. Otherwise, it uses the current kinematics stored in data.
///
/// \return The jacobian of center of mass position of the rigid body system expressed in the world frame (matrix 3 x model.nv).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
PINOCCHIO_DEPRECATED
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool computeSubtreeComs,
const bool updateKinematics);
///
/// \brief Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
/// The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (\sa pinocchio::computeJointJacobians).
......
......@@ -250,21 +250,6 @@ namespace pinocchio
};
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
PINOCCHIO_DEPRECATED
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool computeSubtreeComs,
const bool updateKinematics)
{
if(updateKinematics)
return jacobianCenterOfMass(model,data,q,computeSubtreeComs);
else
return jacobianCenterOfMass(model,data,computeSubtreeComs);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......
//
// Copyright (c) 2016,2018 CNRS
//
#ifndef __pinocchio_finite_differences_hpp__
#define __pinocchio_finite_differences_hpp__
#include "pinocchio/multibody/model.hpp"
namespace pinocchio
{
///
/// \brief Computes the finite difference increments for each degree of freedom according to the current joint configuration.
///
/// \tparam JointCollection Collection of Joint types.
///
/// \input[in] model The model of the kinematic tree.
///
/// \returns The finite difference increments for each degree of freedom.
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
PINOCCHIO_DEPRECATED
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType
finiteDifferenceIncrement(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
}
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/finite-differences.hxx"
#endif // ifndef __pinocchio_finite_differences_hpp__
//
// Copyright (c) 2016,2018 CNRS
//
#ifndef __pinocchio_finite_differences_hxx__
#define __pinocchio_finite_differences_hxx__
#include <limits>
/// @cond DEV
namespace pinocchio
{
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType
finiteDifferenceIncrement(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::TangentVectorType ReturnType;
static Scalar sqrt_eps = math::sqrt(std::numeric_limits<Scalar>::epsilon());
ReturnType fd_increment(ReturnType::Constant(model.nv,sqrt_eps));
return fd_increment;
}
} // namespace pinocchio
/// @endcond