Skip to content
GitLab
Unverified Commit 3e164b1a authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1255 from jcarpent/topic/derivatives 

Add support for LOCAL_WORLD_ALIGNED in kinematics derivatives
parents fec9c9dd e13da1a9
Expand all Hide whitespace changes
Inline Side-by-side
bindings/python/algorithm/algorithms.hpp
View file @ 3e164b1a
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2020 CNRS INRIA
//
#ifndef __pinocchio_python_algorithm_hpp__
#define __pinocchio_python_algorithm_hpp__
#ifndef __pinocchio_python_algorithms_hpp__
#define __pinocchio_python_algorithms_hpp__
#include "pinocchio/bindings/python/fwd.hpp"
#include <boost/python.hpp>
......@@ -34,6 +34,7 @@ namespace pinocchio
void exposeRNEADerivatives();
void exposeABADerivatives();
void exposeKinematicsDerivatives();
void exposeFramesDerivatives();
void exposeCentroidalDerivatives();
void exposeAlgorithms();
......@@ -41,5 +42,5 @@ namespace pinocchio
} // namespace python
} // namespace pinocchio
#endif // ifndef __pinocchio_python_algorithm_hpp__
#endif // ifndef __pinocchio_python_algorithms_hpp__
bindings/python/algorithm/expose-algorithms.cpp
View file @ 3e164b1a
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2020 CNRS INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
......@@ -27,12 +27,13 @@ namespace pinocchio
exposeRegressor();
exposeCholesky();
exposeModelAlgo();
exposeCentroidalDerivatives();
// expose derivative version of the algorithms
exposeRNEADerivatives();
exposeABADerivatives();
exposeKinematicsDerivatives();
exposeFramesDerivatives();
exposeCentroidalDerivatives();
}
} // namespace python
......
bindings/python/algorithm/expose-frames-derivatives.cpp 0 → 100644
View file @ 3e164b1a
//
// Copyright (c) 2020 INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/frames-derivatives.hpp"
#include <boost/python/tuple.hpp>
namespace pinocchio
{
namespace python
{
namespace bp = boost::python;
bp::tuple getFrameVelocityDerivatives_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id,
ReferenceFrame rf)
{
typedef Data::Matrix6x Matrix6x;
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
getFrameVelocityDerivatives(model,data,frame_id,rf,
partial_dq,partial_dv);
return bp::make_tuple(partial_dq,partial_dv);
}
bp::tuple getFrameAccelerationDerivatives_proxy(const Model & model,
Data & data,
const Model::FrameIndex frame_id,
ReferenceFrame rf)
{
typedef Data::Matrix6x Matrix6x;
Matrix6x v_partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x a_partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x a_partial_dv(Matrix6x::Zero(6,model.nv));
Matrix6x a_partial_da(Matrix6x::Zero(6,model.nv));
getFrameAccelerationDerivatives(model,data,frame_id,rf,
v_partial_dq,a_partial_dq,
a_partial_dv,a_partial_da);
return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
}
void exposeFramesDerivatives()
{
using namespace Eigen;
bp::def("getFrameVelocityDerivatives",
getFrameVelocityDerivatives_proxy,
bp::args("model","data","frame_id","reference_frame"),
"Computes the partial derivatives of the spatial velocity of a given frame with respect to\n"
"the joint configuration and velocity and returns them as a tuple.\n"
"The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
"You must first call computForwardKinematicsDerivatives before calling this function.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
"\tdata: data related to the model\n"
"\tframe_id: index of the frame\n"
"\treference_frame: reference frame in which the resulting derivatives are expressed\n");
bp::def("getFrameAccelerationDerivatives",
getFrameAccelerationDerivatives_proxy,
bp::args("model","data","frame_id","reference_frame"),
"Computes the partial derivatives of the spatial acceleration of a given frame with respect to\n"
"the joint configuration, velocity and acceleration and returns them as a tuple.\n"
"The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
"You must first call computForwardKinematicsDerivatives before calling this function.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
"\tdata: data related to the model\n"
"\tframe_id: index of the frame\n"
"\treference_frame: reference frame in which the resulting derivatives are expressed\n");
}
} // namespace python
} // namespace pinocchio
bindings/python/algorithm/expose-kinematics-derivatives.cpp
View file @ 3e164b1a
......@@ -48,6 +48,7 @@ namespace pinocchio
return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
}
Data::Matrix3x getCoMVelocityDerivatives_proxy(const Model & model,
Data & data)
{
......@@ -78,9 +79,9 @@ namespace pinocchio
bp::def("getJointVelocityDerivatives",
getJointVelocityDerivatives_proxy,
bp::args("model","data","joint_id","reference_frame"),
"Computes the partial derivaties of the spatial velocity of a given with respect to\n"
"Computes the partial derivatives of the spatial velocity of a given joint with respect to\n"
"the joint configuration and velocity and returns them as a tuple.\n"
"The Jacobians can be either expressed in the LOCAL frame of the joint or in the WORLD coordinate frame depending on the value of the Reference Frame.\n"
"The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
"You must first call computForwardKinematicsDerivatives before calling this function.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
......@@ -91,9 +92,9 @@ namespace pinocchio
bp::def("getJointAccelerationDerivatives",
getJointAccelerationDerivatives_proxy,
bp::args("model","data","joint_id","reference_frame"),
"Computes the partial derivaties of the spatial acceleration of a given with respect to\n"
"Computes the partial derivatives of the spatial acceleration of a given joint with respect to\n"
"the joint configuration, velocity and acceleration and returns them as a tuple.\n"
"The Jacobians can be either expressed in the LOCAL frame of the joint or in the WORLD coordinate frame depending on the value of the Reference Frame.\n"
"The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
"You must first call computForwardKinematicsDerivatives before calling this function.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
......@@ -104,7 +105,7 @@ namespace pinocchio
bp::def("getCenterOfMassVelocityDerivatives",
getCoMVelocityDerivatives_proxy,
bp::args("model","data"),
"Computes the partial derivaties of the com velocity of a given with respect to\n"
"Computes the partial derivaties of the center of mass velocity with respect to\n"
"the joint configuration.\n"
"You must first call computForwardKinematicsDerivatives and centerOfMass(q,v) "
"before calling this function.\n\n"
......
cmake @ c333a88d
Compare 939e3a72...c333a88d
Subproject commit 939e3a72d73037a270c2dac40d40a8a9d4aa5c08
Subproject commit c333a88decb3e4c0a86947bc6c7f072dc5c5df20
src/algorithm/frames-derivatives.hpp 0 → 100644
View file @ 3e164b1a
//
// Copyright (c) 2020 INRIA
//
#ifndef __pinocchio_algorithm_frames_derivatives_hpp__
#define __pinocchio_algorithm_frames_derivatives_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
namespace pinocchio
{
/**
* @brief Computes the partial derivatives of the frame velocity quantity with respect to q and v.
* You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities.
*
* @tparam JointCollection Collection of Joint types.
* @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
* @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[in] rf Reference frame in which the velocity is expressed.
* @param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$.
* @param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ \dot{q} \f$.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2>
void
getFrameVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
/**
* @brief Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a.
* You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities.
* It is important to notice that a direct outcome (for free) of this algo is v_partial_dq and v_partial_dv which is equal to a_partial_da.
*
* @tparam JointCollection Collection of Joint types.
* @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
* @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
* @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
* @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[in] rf Reference frame in which the velocity is expressed.
* @param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{q} \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \ddot{q} \f$.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
void
getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
/**
* @brief Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a.
* You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities.
* It is important to notice that a direct outcome (for free) of this algo is v_partial_dq and v_partial_dv which is equal to a_partial_da.
*
* @tparam JointCollection Collection of Joint types.
* @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
* @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector.
* @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
* @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
* @tparam Matrix6xOut5 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[in] rf Reference frame in which the velocity is expressed.
* @param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$.
* @param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ \dot{q} \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{q} \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \ddot{q} \f$.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
void
getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da);
}
#include "pinocchio/algorithm/frames-derivatives.hxx"
#endif // ifndef __pinocchio_algorithm_frames_derivatives_hpp__
src/algorithm/frames-derivatives.hxx 0 → 100644
View file @ 3e164b1a
//
// Copyright (c) 2020 INRIA
//
#ifndef __pinocchio_algorithm_frames_derivatives_hxx__
#define __pinocchio_algorithm_frames_derivatives_hxx__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
namespace pinocchio
{
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2>
void
getFrameVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Data::Matrix6x Matrix6x;
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Matrix6x);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_partial_dv.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames");
typedef typename Model::Frame Frame;
typedef typename Data::Motion Motion;
const Frame & frame = model.frames[frame_id];
const JointIndex joint_id = frame.parent;
Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq);
Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv);
getJointVelocityDerivatives(model,data,joint_id,rf,
v_partial_dq_,v_partial_dv_);
// Update frame placement
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[joint_id] * frame.placement;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1;
typedef MotionRef<ColsBlockOut1> MotionOut1;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2;
typedef MotionRef<ColsBlockOut2> MotionOut2;
Motion v_tmp;
const typename SE3::Vector3 trans = data.oMi[joint_id].rotation() * frame.placement.translation();
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
switch (rf)
{
case WORLD:
// Do nothing
break;
case LOCAL_WORLD_ALIGNED:
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id])
{
MotionOut1 m1(v_partial_dq_.col(col_id));
m1.linear() -= trans.cross(m1.angular());
MotionOut2 m2(v_partial_dv_.col(col_id));
m2.linear() -= trans.cross(m2.angular());
}
break;
case LOCAL:
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id])
{
v_tmp = v_partial_dq_.col(col_id);
MotionOut1(v_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp);
v_tmp = v_partial_dv_.col(col_id);
MotionOut2(v_partial_dv_.col(col_id)) = frame.placement.actInv(v_tmp);
}
break;
default:
break;
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
void
getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Data::Matrix6x Matrix6x;
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4,Matrix6x);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_dv.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_da.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames");
typedef typename Model::Frame Frame;
typedef typename Data::Motion Motion;
const Frame & frame = model.frames[frame_id];
const JointIndex joint_id = frame.parent;
Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq);
Matrix6xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,a_partial_dq);
Matrix6xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dv);
Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_da);
getJointAccelerationDerivatives(model,data,joint_id,rf,
v_partial_dq_,a_partial_dq_,a_partial_dv_,a_partial_da_);
// Update frame placement
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[joint_id] * frame.placement;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1;
typedef MotionRef<ColsBlockOut1> MotionOut1;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2;
typedef MotionRef<ColsBlockOut2> MotionOut2;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut3>::Type ColsBlockOut3;
typedef MotionRef<ColsBlockOut3> MotionOut3;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut4>::Type ColsBlockOut4;
typedef MotionRef<ColsBlockOut4> MotionOut4;
Motion v_tmp;
const typename SE3::Vector3 trans = data.oMi[joint_id].rotation() * frame.placement.translation();
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
switch (rf)
{
case WORLD:
// Do nothing
break;
case LOCAL_WORLD_ALIGNED:
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id])
{
MotionOut1 m1(v_partial_dq_.col(col_id));
m1.linear() -= trans.cross(m1.angular());
MotionOut2 m2(a_partial_dq_.col(col_id));
m2.linear() -= trans.cross(m2.angular());
MotionOut3 m3(a_partial_dv_.col(col_id));
m3.linear() -= trans.cross(m3.angular());
MotionOut4 m4(a_partial_da_.col(col_id));
m4.linear() -= trans.cross(m4.angular());
}
break;
case LOCAL:
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id])
{
v_tmp = v_partial_dq_.col(col_id);
MotionOut1(v_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp);
v_tmp = a_partial_dq_.col(col_id);
MotionOut2(a_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp);
v_tmp = a_partial_dv_.col(col_id);
MotionOut3(a_partial_dv_.col(col_id)) = frame.placement.actInv(v_tmp);
v_tmp = a_partial_da_.col(col_id);
MotionOut4(a_partial_da_.col(col_id)) = frame.placement.actInv(v_tmp);
}
break;
default:
break;
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
inline void getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
{
getFrameAccelerationDerivatives(model,data,
frame_id,rf,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_dv),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5,a_partial_da));
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv) = a_partial_da;
}
}
#endif // ifndef __pinocchio_algorithm_frames_derivatives_hxx__
src/algorithm/jacobian.hxx
View file @ 3e164b1a
......@@ -120,6 +120,33 @@ namespace pinocchio
namespace details
{
template<typename Scalar, int Options, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
void translateJointJacobian(const SE3Tpl<Scalar,Options> & placement,
const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.cols() == Jout.cols());
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.rows() == 6);
Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);
typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
typedef const MotionRef<ConstColXprIn> MotionIn;
typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
typedef MotionRef<ColXprOut> MotionOut;
for(Eigen::DenseIndex j=0; j < Jin.cols(); ++j)
{
MotionIn v_in(Jin.col(j));
MotionOut v_out(Jout_.col(j));
v_out = v_in;
v_out.linear() -= placement.translation().cross(v_in.angular());
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
......
src/algorithm/kinematics-derivatives.hpp
View file @ 3e164b1a
......@@ -2,8 +2,8 @@
// Copyright (c) 2017-2019 CNRS INRIA
//
#ifndef __pinocchio_kinematics_derivatives_hpp__
#define __pinocchio_kinematics_derivatives_hpp__
#ifndef __pinocchio_algorithm_kinematics_derivatives_hpp__
#define __pinocchio_algorithm_kinematics_derivatives_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
......@@ -54,7 +54,7 @@ namespace pinocchio
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2>
inline void getJointVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -83,7 +83,7 @@ namespace pinocchio
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -116,7 +116,7 @@ namespace pinocchio
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -235,4 +235,4 @@ namespace pinocchio
#include "pinocchio/algorithm/kinematics-derivatives.hxx"
#endif // ifndef __pinocchio_kinematics_derivatives_hpp__
#endif // ifndef __pinocchio_algorithm_kinematics_derivatives_hpp__
src/algorithm/kinematics-derivatives.hxx
View file @ 3e164b1a
//
// Copyright (c) 2017-2019 CNRS INRIA
// Copyright (c) 2017-2020 CNRS INRIA
//
#ifndef __pinocchio_kinematics_derivatives_hxx__
#define __pinocchio_kinematics_derivatives_hxx__
#ifndef __pinocchio_algorithm_kinematics_derivatives_hxx__
#define __pinocchio_algorithm_kinematics_derivatives_hxx__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"