Commit de18e056 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

Merge pull request #132 from jcarpent/devel

Add forward dynamics with contacts algorithm 
parents 9d42031d 3c047ae3
...@@ -144,6 +144,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS ...@@ -144,6 +144,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/cholesky.hxx algorithm/cholesky.hxx
algorithm/kinematics.hpp algorithm/kinematics.hpp
algorithm/kinematics.hxx algorithm/kinematics.hxx
algorithm/dynamics.hpp
algorithm/center-of-mass.hpp algorithm/center-of-mass.hpp
algorithm/center-of-mass.hxx algorithm/center-of-mass.hxx
algorithm/non-linear-effects.hpp algorithm/non-linear-effects.hpp
......
Subproject commit 7cdd2146508a3431adb8e5a0d4233704080299cd Subproject commit 9f9a90007704d679c631431ab36189f7ba61f323
...@@ -40,6 +40,8 @@ namespace se3 ...@@ -40,6 +40,8 @@ namespace se3
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// ///
/// \return A reference to the upper triangular matrix \f$U\f$.
///
inline const Eigen::MatrixXd & inline const Eigen::MatrixXd &
decompose(const Model & model, decompose(const Model & model,
Data & data); Data & data);
...@@ -54,7 +56,7 @@ namespace se3 ...@@ -54,7 +56,7 @@ namespace se3
/// ///
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[inout] y The input vector to inverse which also contains the result \f$x\f$ of the inversion. /// \param[inout] y The input matrix to inverse which also contains the result \f$x\f$ of the inversion.
/// ///
template<typename Mat> template<typename Mat>
Mat & solve(const Model & model, const Data & data, Mat & solve(const Model & model, const Data & data,
...@@ -65,7 +67,7 @@ namespace se3 ...@@ -65,7 +67,7 @@ namespace se3
/// ///
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input vector to multiply with data.M and storing the result. /// \param[inout] v The input matrix to multiply with data.M and storing the result.
/// \param[in] usingCholesky If true, use the Cholesky decomposition stored in data. Exploit the sparsity of the kinematic tree. /// \param[in] usingCholesky If true, use the Cholesky decomposition stored in data. Exploit the sparsity of the kinematic tree.
/// ///
/// \return A reference to the result of \f$ Mv \f$. /// \return A reference to the result of \f$ Mv \f$.
...@@ -80,7 +82,7 @@ namespace se3 ...@@ -80,7 +82,7 @@ namespace se3
/// ///
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input vector to multiply with data.U and also storing the result. /// \param[inout] v The input matrix to multiply with data.U and also storing the result.
/// ///
/// \return A reference to the result of \f$ Uv \f$ stored in v. /// \return A reference to the result of \f$ Uv \f$ stored in v.
/// ///
...@@ -94,7 +96,7 @@ namespace se3 ...@@ -94,7 +96,7 @@ namespace se3
/// ///
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input vector to multiply with data.U.tranpose() and also storing the result. /// \param[inout] v The input matrix to multiply with data.U.tranpose() and also storing the result.
/// ///
/// \return A reference to the result of \f$ U^{\top}v \f$ stored in v. /// \return A reference to the result of \f$ U^{\top}v \f$ stored in v.
/// ///
...@@ -108,7 +110,7 @@ namespace se3 ...@@ -108,7 +110,7 @@ namespace se3
/// ///
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input vector to multiply with data.U^{-1} and also storing the result. /// \param[inout] v The input matrix to multiply with data.U^{-1} and also storing the result.
/// ///
/// \return A reference to the result of \f$ U^{-1}v \f$ stored in v. /// \return A reference to the result of \f$ U^{-1}v \f$ stored in v.
/// ///
...@@ -124,7 +126,7 @@ namespace se3 ...@@ -124,7 +126,7 @@ namespace se3
/// ///
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input vector to multiply with data.U^{-\top} and also storing the result. /// \param[inout] v The input matrix to multiply with data.U^{-\top} and also storing the result.
/// ///
/// \return A reference to the result of \f$ U^{-\top}v \f$ stored in v. /// \return A reference to the result of \f$ U^{-\top}v \f$ stored in v.
/// ///
...@@ -140,7 +142,7 @@ namespace se3 ...@@ -140,7 +142,7 @@ namespace se3
/// ///
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[inout] v The input vector to multiply with data.M^{-1} and also storing the result. /// \param[inout] v The input matrix to multiply with data.M^{-1} and also storing the result.
/// ///
/// \return A reference to the result of \f$ M^{-1}v \f$ stored in v. /// \return A reference to the result of \f$ M^{-1}v \f$ stored in v.
/// ///
......
...@@ -71,13 +71,13 @@ namespace se3 ...@@ -71,13 +71,13 @@ namespace se3
const Data & data, const Data & data,
Eigen::MatrixBase<Mat> & v) Eigen::MatrixBase<Mat> & v)
{ {
assert(v.size() == model.nv); assert(v.rows() == model.nv);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
const Eigen::MatrixXd & U = data.U; const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow; const std::vector<int> & nvt = data.nvSubtree_fromRow;
for(int k=0;k < model.nv-1;++k) // You can stop one step before nv for(int k=0;k < model.nv-1;++k) // You can stop one step before nv
v[k] += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1); v.row(k) += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.middleRows(k+1,nvt[(Model::Index)k]-1);
return v.derived(); return v.derived();
} }
...@@ -88,13 +88,12 @@ namespace se3 ...@@ -88,13 +88,12 @@ namespace se3
const Data & data, const Data & data,
Eigen::MatrixBase<Mat> & v) Eigen::MatrixBase<Mat> & v)
{ {
assert(v.size() == model.nv); assert(v.rows() == model.nv);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
const Eigen::MatrixXd & U = data.U; const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow; const std::vector<int> & nvt = data.nvSubtree_fromRow;
for( int i=model.nv-2;i>=0;--i ) // You can start from nv-2 (no child in nv-1) for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1)
v.segment(i+1,nvt[(Model::Index)i]-1) += U.row(i).segment(i+1,nvt[(Model::Index)i]-1).transpose()*v[i]; v.middleRows(k+1,nvt[(Model::Index)k]-1) += U.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k);
return v.derived(); return v.derived();
} }
...@@ -109,14 +108,13 @@ namespace se3 ...@@ -109,14 +108,13 @@ namespace se3
{ {
/* We search y s.t. v = U y. /* We search y s.t. v = U y.
* For any k, v_k = y_k + U_{k,k+1:} y_{k+1:} */ * For any k, v_k = y_k + U_{k,k+1:} y_{k+1:} */
assert(v.size() == model.nv); assert(v.rows() == model.nv);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
const Eigen::MatrixXd & U = data.U; const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow; const std::vector<int> & nvt = data.nvSubtree_fromRow;
for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1) for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1)
v[k] -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1); v.row(k) -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.middleRows(k+1,nvt[(Model::Index)k]-1);
return v.derived(); return v.derived();
} }
...@@ -127,13 +125,12 @@ namespace se3 ...@@ -127,13 +125,12 @@ namespace se3
{ {
/* We search y s.t. v = U' y. /* We search y s.t. v = U' y.
* For any k, v_k = y_k + sum_{m \in parent{k}} U(m,k) v(k). */ * For any k, v_k = y_k + sum_{m \in parent{k}} U(m,k) v(k). */
assert(v.size() == model.nv); assert(v.rows() == model.nv);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
const Eigen::MatrixXd & U = data.U; const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow; const std::vector<int> & nvt = data.nvSubtree_fromRow;
for( int i=0;i<model.nv-1;++i ) // You can stop one step before nv. for( int k=0;k<model.nv-1;++k ) // You can stop one step before nv.
v.segment(i+1,nvt[(Model::Index)i]-1) -= U.row(i).segment(i+1,nvt[(Model::Index)i]-1).transpose()*v[i]; v.middleRows(k+1,nvt[(Model::Index)k]-1) -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k);
return v.derived(); return v.derived();
} }
...@@ -145,8 +142,7 @@ namespace se3 ...@@ -145,8 +142,7 @@ namespace se3
const Data & data, const Data & data,
const Eigen::MatrixBase<Mat> & v) const Eigen::MatrixBase<Mat> & v)
{ {
assert(v.size() == model.nv); assert(v.rows() == model.nv);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
const Eigen::MatrixXd & M = data.M; const Eigen::MatrixXd & M = data.M;
const std::vector<int> & nvt = data.nvSubtree_fromRow; const std::vector<int> & nvt = data.nvSubtree_fromRow;
...@@ -154,8 +150,8 @@ namespace se3 ...@@ -154,8 +150,8 @@ namespace se3
for(int k=model.nv-1;k>=0;--k) for(int k=model.nv-1;k>=0;--k)
{ {
res[k] = M.row(k).segment(k,nvt[(Model::Index)k]) * v.segment(k,nvt[(Model::Index)k]); res.row(k) = M.row(k).segment(k,nvt[(Model::Index)k]) * v.middleRows(k,nvt[(Model::Index)k]);
res.segment(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v[k]; res.middleRows(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k);
} }
return res; return res;
...@@ -167,7 +163,7 @@ namespace se3 ...@@ -167,7 +163,7 @@ namespace se3
Eigen::MatrixBase<Mat> & v) Eigen::MatrixBase<Mat> & v)
{ {
Utv(model,data,v); Utv(model,data,v);
for( int k=0;k<model.nv;++k ) v[k] *= data.D[k]; for( int k=0;k<model.nv;++k ) v.row(k) *= data.D[k];
return Uv(model,data,v); return Uv(model,data,v);
} }
} // internal } // internal
...@@ -188,7 +184,7 @@ namespace se3 ...@@ -188,7 +184,7 @@ namespace se3
Eigen::MatrixBase<Mat> & v) Eigen::MatrixBase<Mat> & v)
{ {
Uiv(model,data,v); Uiv(model,data,v);
for(int k=0;k<model.nv;++k) v[k] /= data.D[k]; for(int k=0;k<model.nv;++k) v.row(k) /= data.D[k];
return Utiv(model,data,v); return Utiv(model,data,v);
} }
......
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_dynamics_hpp__
#define __se3_dynamics_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/simulation/compute-all-terms.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include <Eigen/Cholesky>
namespace se3
{
///
/// \brief Compute the forward dynamics with contact constraints.
/// \note It computes the following problem: <BR>
/// <CENTER> \f$ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\
/// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$ </CENTER> <BR>
/// where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints),
/// \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \gamma \f$ is the constraint drift.
///
/// \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] v The joint velocity (vector dim model.nv).
/// \param[in] tau The joint torque vector (dim model.nv).
/// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).
/// \param[in] gamma The drift of the constraints (dim nb_constraints).
/// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data.
///
/// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda vector.
///
inline const Eigen::VectorXd & forwardDynamics(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau,
const Eigen::MatrixXd & J,
const Eigen::VectorXd & gamma,
const bool updateKinematics = true
)
{
assert(q.size() == model.nq);
assert(v.size() == model.nv);
assert(tau.size() == model.nv);
assert(J.cols() == model.nv);
assert(J.rows() == gamma.size());
Eigen::VectorXd & a = data.ddq;
Eigen::VectorXd & lambda = data.lambda;
if (updateKinematics)
computeAllTerms(model, data, q, v);
// Compute the UDUt decomposition of data.M
cholesky::decompose(model, data);
// Compute the dynamic drift (control - nle)
data.torque_residual = tau - data.nle;
cholesky::solve(model, data, data.torque_residual);
data.sDUiJt = J.transpose();
// Compute U^-1 * J.T
cholesky::Uiv(model, data, data.sDUiJt);
for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);
data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
data.llt_JMinvJt.compute(data.JMinvJt);
// Compute the Lagrange Multipliers
lambda = -gamma -J*data.torque_residual;
data.llt_JMinvJt.solveInPlace (lambda);
// Compute the joint acceleration
a = J.transpose() * lambda;
cholesky::solve (model, data, a);
a += data.torque_residual;
return a;
}
} // namespace se3
#endif // ifndef __se3_dynamics_hpp__
...@@ -29,6 +29,7 @@ ...@@ -29,6 +29,7 @@
#include "pinocchio/multibody/joint/joint-variant.hpp" #include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/tools/string-generator.hpp" #include "pinocchio/tools/string-generator.hpp"
#include <iostream> #include <iostream>
#include <Eigen/Cholesky>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia)
...@@ -472,6 +473,23 @@ namespace se3 ...@@ -472,6 +473,23 @@ namespace se3
/// \brief Potential energy of the model. /// \brief Potential energy of the model.
double potential_energy; double potential_energy;
// Temporary variables used in forward dynamics
/// \brief Inverse of the operational-space inertia matrix
Eigen::MatrixXd JMinvJt;
/// \brief Cholesky decompostion of \JMinvJt.
Eigen::LLT<Eigen::MatrixXd> llt_JMinvJt;
/// \brief Lagrange Multipliers corresponding to contact forces.
Eigen::VectorXd lambda;
/// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
Eigen::MatrixXd sDUiJt;
/// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
Eigen::VectorXd torque_residual;
/// ///
/// \brief Default constructor of se3::Data from a se3::Model. /// \brief Default constructor of se3::Data from a se3::Model.
/// ///
......
...@@ -279,6 +279,11 @@ namespace se3 ...@@ -279,6 +279,11 @@ namespace se3
,velocityLimit(ref.nv) ,velocityLimit(ref.nv)
,lowerPositionLimit(ref.nq) ,lowerPositionLimit(ref.nq)
,upperPositionLimit(ref.nq) ,upperPositionLimit(ref.nq)
,JMinvJt()
,llt_JMinvJt()
,lambda()
,sDUiJt(ref.nv,ref.nv)
,torque_residual(ref.nv)
{ {
/* Create data strcture associated to the joints */ /* Create data strcture associated to the joints */
for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i) for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i)
......
...@@ -28,6 +28,7 @@ ...@@ -28,6 +28,7 @@
#include "pinocchio/algorithm/non-linear-effects.hpp" #include "pinocchio/algorithm/non-linear-effects.hpp"
#include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/aba.hpp" #include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/dynamics.hpp"
#include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/operational-frames.hpp" #include "pinocchio/algorithm/operational-frames.hpp"
...@@ -94,6 +95,19 @@ namespace se3 ...@@ -94,6 +95,19 @@ namespace se3
aba(*model,*data,q,v,tau); aba(*model,*data,q,v,tau);
return data->ddq; return data->ddq;
} }
static Eigen::MatrixXd fd_llt_proxy(const ModelHandler & model,
DataHandler & data,
const VectorXd_fx & q,
const VectorXd_fx & v,
const VectorXd_fx & tau,
const eigenpy::MatrixXd_fx & J,
const VectorXd_fx & gamma,
const bool update_kinematics = true)
{
forwardDynamics(*model,*data,q,v,tau,J,gamma,update_kinematics);
return data->ddq;
}
static SE3::Vector3 static SE3::Vector3
com_0_proxy(const ModelHandler& model, com_0_proxy(const ModelHandler& model,
...@@ -327,6 +341,16 @@ namespace se3 ...@@ -327,6 +341,16 @@ namespace se3
"Joint torque tau (size Model::nv)"), "Joint torque tau (size Model::nv)"),
"Compute ABA, put the result in Data::ddq and return it."); "Compute ABA, put the result in Data::ddq and return it.");
bp::def("forwardDynamics",fd_llt_proxy,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
"Joint torque tau (size Model::nv)",
"Contact Jacobian J (size nb_constraint * Model::nv)",
"Contact drift gamma (size nb_constraint)",
"Update kinematics (if true, it updates the dynamic variable according to the current state)"),
"Solve the forward dynamics problem with contacts, put the result in Data::ddq and return it.");
bp::def("centerOfMass",com_0_proxy, bp::def("centerOfMass",com_0_proxy,
bp::args("Model","Data", bp::args("Model","Data",
"Configuration q (size Model::nq)", "Configuration q (size Model::nq)",
......
...@@ -112,6 +112,8 @@ namespace se3 ...@@ -112,6 +112,8 @@ namespace se3
.ADD_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)") .ADD_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
.ADD_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)") .ADD_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces")
; ;
} }
...@@ -159,6 +161,8 @@ namespace se3 ...@@ -159,6 +161,8 @@ namespace se3
IMPL_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)") IMPL_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
IMPL_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)") IMPL_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces")
/* --- Expose --------------------------------------------------------- */ /* --- Expose --------------------------------------------------------- */
static void expose() static void expose()
{ {
......
// //
// Copyright (c) 2015 CNRS // Copyright (c) 2015-2016 CNRS
// //
// This file is part of Pinocchio // This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it // Pinocchio is free software: you can redistribute it
...@@ -109,9 +109,9 @@ namespace se3 ...@@ -109,9 +109,9 @@ namespace se3
return derived().__equal__(other); return derived().__equal__(other);
} }
bool isApprox (const Derived_t & other) const bool isApprox (const Derived_t & other, const Scalar_t & prec = Eigen::NumTraits<Scalar_t>::dummy_precision()) const
{ {
return derived().isApprox_impl(other); return derived().isApprox_impl(other, prec);
} }
friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X) friend std::ostream & operator << (std::ostream & os,const SE3Base<Derived> & X)
...@@ -164,12 +164,15 @@ namespace se3 ...@@ -164,12 +164,15 @@ namespace se3
SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p) SE3Tpl(const Eigen::MatrixBase<M3> & R, const Eigen::MatrixBase<v3> & p)
: rot(R), trans(p) : rot(R), trans(p)
{ {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(v3,3)
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M3,3,3)
} }
template<typename M4> template<typename M4>
SE3Tpl(const Eigen::MatrixBase<M4> & m) SE3Tpl(const Eigen::MatrixBase<M4> & m)
: rot(m.template block<3,3>(LINEAR,LINEAR)), trans(m.template block<3,1>(LINEAR,ANGULAR)) : rot(m.template block<3,3>(LINEAR,LINEAR)), trans(m.template block<3,1>(LINEAR,ANGULAR))
{ {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M4,4,4);
} }
SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {} SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {}
...@@ -216,8 +219,7 @@ namespace se3 ...@@ -216,8 +219,7 @@ namespace se3
return *this; return *this;
} }
public:
Matrix4 toHomogeneousMatrix_impl() const Matrix4 toHomogeneousMatrix_impl() const
{ {
Matrix4 M; Matrix4 M;
...@@ -244,7 +246,6 @@ namespace se3 ...@@ -244,7 +246,6 @@ namespace se3
return M; return M;
} }
void disp_impl(std::ostream & os) const void disp_impl(std::ostream & os) const
{ {
os << " R =\n" << rot << std::endl os << " R =\n" << rot << std::endl
...@@ -265,8 +266,8 @@ namespace se3 ...@@ -265,8 +266,8 @@ namespace se3
return d.se3ActionInverse(*this); return d.se3ActionInverse(*this);
} }