Commit d8b2196e authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[pinocchio-v2.1] Apply pinocchio v2.1.0 API changes.

parent 7036c61c
......@@ -43,7 +43,7 @@ namespace tsid
typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality;
typedef se3::SE3 SE3;
typedef pinocchio::SE3 SE3;
Contact6d(const std::string & name,
RobotWrapper & robot,
......
......@@ -42,7 +42,7 @@ namespace tsid
typedef math::ConstRefVector ConstRefVector;
typedef math::Matrix Matrix;
typedef tasks::TaskMotion TaskMotion;
typedef se3::Data Data;
typedef pinocchio::Data Data;
typedef robots::RobotWrapper RobotWrapper;
ContactBase(const std::string & name,
......
......@@ -72,7 +72,7 @@ namespace tsid
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef se3::Data Data;
typedef pinocchio::Data Data;
typedef math::Vector Vector;
typedef math::Matrix Matrix;
typedef math::ConstRefVector ConstRefVector;
......
......@@ -40,7 +40,7 @@ namespace tsid
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef se3::Data Data;
typedef pinocchio::Data Data;
typedef math::Vector Vector;
typedef math::RefVector RefVector;
typedef math::ConstRefVector ConstRefVector;
......
......@@ -81,18 +81,18 @@ namespace tsid
/**
* Convert the input SE3 object to a 7D vector of floats [X,Y,Z,Q1,Q2,Q3,Q4].
*/
void SE3ToXYZQUAT(const se3::SE3 & M, RefVector xyzQuat);
void SE3ToXYZQUAT(const pinocchio::SE3 & M, RefVector xyzQuat);
/**
* Convert the input SE3 object to a 12D vector of floats [X,Y,Z,R11,R12,R13,R14,...].
*/
void SE3ToVector(const se3::SE3 & M, RefVector vec);
void SE3ToVector(const pinocchio::SE3 & M, RefVector vec);
void vectorToSE3(RefVector vec, se3::SE3 & M);
void vectorToSE3(RefVector vec, pinocchio::SE3 & M);
void errorInSE3 (const se3::SE3 & M,
const se3::SE3 & Mdes,
se3::Motion & error);
void errorInSE3 (const pinocchio::SE3 & M,
const pinocchio::SE3 & Mdes,
pinocchio::Motion & error);
void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd> & svd,
ConstRefVector b,
......
......@@ -28,6 +28,7 @@
#include <string>
#include <vector>
namespace tsid
{
namespace robots
......@@ -41,11 +42,11 @@ namespace tsid
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Scalar Scalar;
typedef se3::Model Model;
typedef se3::Data Data;
typedef se3::Motion Motion;
typedef se3::Frame Frame;
typedef se3::SE3 SE3;
typedef pinocchio::Model Model;
typedef pinocchio::Data Data;
typedef pinocchio::Motion Motion;
typedef pinocchio::Frame Frame;
typedef pinocchio::SE3 SE3;
typedef math::Vector Vector;
typedef math::Vector3 Vector3;
typedef math::Vector6 Vector6;
......@@ -61,7 +62,7 @@ namespace tsid
RobotWrapper(const std::string & filename,
const std::vector<std::string> & package_dirs,
const se3::JointModelVariant & rootJoint,
const pinocchio::JointModelVariant & rootJoint,
bool verbose=false);
virtual int nq() const;
......
......@@ -81,10 +81,10 @@ namespace tsid
{ return aligned_pair<T1,T2>(t1,t2); }
typedef se3::container::aligned_vector< aligned_pair<double, math::ConstraintBase*> > ConstraintLevel;
typedef se3::container::aligned_vector< aligned_pair<double, const math::ConstraintBase*> > ConstConstraintLevel;
typedef se3::container::aligned_vector<ConstraintLevel> HQPData;
typedef se3::container::aligned_vector<ConstConstraintLevel> ConstHQPData;
typedef pinocchio::container::aligned_vector< aligned_pair<double, math::ConstraintBase*> > ConstraintLevel;
typedef pinocchio::container::aligned_vector< aligned_pair<double, const math::ConstraintBase*> > ConstConstraintLevel;
typedef pinocchio::container::aligned_vector<ConstraintLevel> HQPData;
typedef pinocchio::container::aligned_vector<ConstConstraintLevel> ConstHQPData;
}
......
......@@ -40,7 +40,7 @@ namespace tsid
typedef math::ConstraintBase ConstraintBase;
typedef math::ConstRefVector ConstRefVector;
typedef se3::Data Data;
typedef pinocchio::Data Data;
typedef robots::RobotWrapper RobotWrapper;
TaskBase(const std::string & name,
......
......@@ -37,7 +37,7 @@ namespace tsid
typedef math::Vector Vector;
typedef math::VectorXi VectorXi;
typedef math::ConstraintEquality ConstraintEquality;
typedef se3::Data Data;
typedef pinocchio::Data Data;
TaskJointPosture(const std::string & name,
RobotWrapper & robot);
......
......@@ -39,10 +39,10 @@ namespace tsid
typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector;
typedef math::ConstraintEquality ConstraintEquality;
typedef se3::Data Data;
typedef se3::Data::Matrix6x Matrix6x;
typedef se3::Motion Motion;
typedef se3::SE3 SE3;
typedef pinocchio::Data Data;
typedef pinocchio::Data::Matrix6x Matrix6x;
typedef pinocchio::Motion Motion;
typedef pinocchio::SE3 SE3;
TaskSE3Equality(const std::string & name,
RobotWrapper & robot,
......
......@@ -32,7 +32,7 @@ namespace tsid
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef se3::SE3 SE3;
typedef pinocchio::SE3 SE3;
TrajectorySE3Constant(const std::string & name);
......
......@@ -134,7 +134,7 @@ void Contact6d:: updateForceGeneratorMatrix()
for(int i=0; i<4; i++)
{
m_forceGenMat.block<3,3>(0, i*3).setIdentity();
m_forceGenMat.block<3,3>(3, i*3) = se3::skew(m_contactPoints.col(i));
m_forceGenMat.block<3,3>(3, i*3) = pinocchio::skew(m_contactPoints.col(i));
}
}
......
......@@ -25,7 +25,7 @@ using namespace tasks;
using namespace contacts;
using namespace solvers;
typedef se3::Data Data;
typedef pinocchio::Data Data;
TaskLevel::TaskLevel(tasks::TaskBase & task,
unsigned int priority):
......
......@@ -22,14 +22,14 @@ namespace tsid
namespace math
{
void SE3ToXYZQUAT(const se3::SE3 & M, RefVector xyzQuat)
void SE3ToXYZQUAT(const pinocchio::SE3 & M, RefVector xyzQuat)
{
assert(xyzQuat.size()==7);
xyzQuat.head<3>() = M.translation();
xyzQuat.tail<4>() = Eigen::Quaterniond(M.rotation()).coeffs();
}
void SE3ToVector(const se3::SE3 & M, RefVector vec)
void SE3ToVector(const pinocchio::SE3 & M, RefVector vec)
{
assert(vec.size()==12);
vec.head<3>() = M.translation();
......@@ -37,7 +37,7 @@ namespace tsid
vec.tail<9>() = Eigen::Map<const Vector9>(&M.rotation()(0), 9);
}
void vectorToSE3(RefVector vec, se3::SE3 & M)
void vectorToSE3(RefVector vec, pinocchio::SE3 & M)
{
assert(vec.size()==12);
M.translation( vec.head<3>() );
......@@ -45,11 +45,11 @@ namespace tsid
M.rotation( Eigen::Map<const Matrix3>(&vec(3), 3, 3) );
}
void errorInSE3 (const se3::SE3 & M,
const se3::SE3 & Mdes,
se3::Motion & error)
void errorInSE3 (const pinocchio::SE3 & M,
const pinocchio::SE3 & Mdes,
pinocchio::Motion & error)
{
error = se3::log6(Mdes.inverse() * M);
error = pinocchio::log6(Mdes.inverse() * M);
}
void solveWithDampingFromSvd(Eigen::JacobiSVD<Eigen::MatrixXd> & svd,
......
......@@ -24,7 +24,7 @@
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/frames.hpp>
using namespace se3;
using namespace pinocchio;
using namespace tsid::math;
namespace tsid
......@@ -37,7 +37,7 @@ namespace tsid
bool verbose)
: m_verbose(verbose)
{
se3::urdf::buildModel(filename, m_model, m_verbose);
pinocchio::urdf::buildModel(filename, m_model, m_verbose);
m_model_filename = filename;
m_rotor_inertias.setZero(m_model.nv);
m_gear_ratios.setZero(m_model.nv);
......@@ -47,11 +47,11 @@ namespace tsid
RobotWrapper::RobotWrapper(const std::string & filename,
const std::vector<std::string> & ,
const se3::JointModelVariant & rootJoint,
const pinocchio::JointModelVariant & rootJoint,
bool verbose)
: m_verbose(verbose)
{
se3::urdf::buildModel(filename, rootJoint, m_model, m_verbose);
pinocchio::urdf::buildModel(filename, rootJoint, m_model, m_verbose);
m_model_filename = filename;
m_rotor_inertias.setZero(m_model.nv-6);
m_gear_ratios.setZero(m_model.nv-6);
......@@ -67,13 +67,14 @@ namespace tsid
void RobotWrapper::computeAllTerms(Data & data, const Vector & q, const Vector & v) const
{
se3::computeAllTerms(m_model, data, q, v);
pinocchio::computeAllTerms(m_model, data, q, v);
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
// computeAllTerms does not compute the com acceleration, so we need to call centerOfMass
se3::centerOfMass<true,true,true>(m_model, data, false);
se3::updateFramePlacements(m_model, data);
se3::centerOfMass(m_model, data, q, v, Eigen::VectorXd::Zero(nv()));
// Check this line, calling with zero acceleration at the last phase compute the CoM acceleration.
// pinocchio::centerOfMass(m_model, data, q,v,false);
pinocchio::framesForwardKinematics(m_model, data);
pinocchio::centerOfMass(m_model, data, q, v, Eigen::VectorXd::Zero(nv()));
}
const Vector & RobotWrapper::rotor_inertias() const
......@@ -170,14 +171,22 @@ namespace tsid
const Model::JointIndex index,
Data::Matrix6x & J) const
{
<<<<<<< 7036c61c0ac04313b92b5f31544ab4986d80bba0
return se3::getJointJacobian<se3::WORLD>(m_model, data, index, J);
=======
return pinocchio::getFrameJacobian(m_model, data, index,pinocchio::WORLD,J);
>>>>>>> [pinocchio-v2.1] Apply pinocchio v2.1.0 API changes.
}
void RobotWrapper::jacobianLocal(const Data & data,
const Model::JointIndex index,
Data::Matrix6x & J) const
{
<<<<<<< 7036c61c0ac04313b92b5f31544ab4986d80bba0
return se3::getJointJacobian<se3::LOCAL>(m_model, data, index, J);
=======
return pinocchio::getFrameJacobian(m_model, data, index,pinocchio::LOCAL, J);
>>>>>>> [pinocchio-v2.1] Apply pinocchio v2.1.0 API changes.
}
SE3 RobotWrapper::framePosition(const Data & data,
......@@ -249,27 +258,35 @@ namespace tsid
const Model::FrameIndex index,
Data::Matrix6x & J) const
{
<<<<<<< 7036c61c0ac04313b92b5f31544ab4986d80bba0
return se3::getJointJacobian<se3::WORLD>(m_model, data, m_model.frames[index].parent, J);
=======
return pinocchio::getFrameJacobian(m_model, data, m_model.frames[index].parent,pinocchio::WORLD, J);
>>>>>>> [pinocchio-v2.1] Apply pinocchio v2.1.0 API changes.
}
void RobotWrapper::frameJacobianLocal(const Data & data,
const Model::FrameIndex index,
Data::Matrix6x & J) const
{
<<<<<<< 7036c61c0ac04313b92b5f31544ab4986d80bba0
return se3::getFrameJacobian<se3::LOCAL>(m_model, data, index, J);
=======
return pinocchio::getFrameJacobian(m_model, data, index, pinocchio::LOCAL,J);
>>>>>>> [pinocchio-v2.1] Apply pinocchio v2.1.0 API changes.
}
// const Vector3 & com(Data & data,const Vector & q,
// const bool computeSubtreeComs = true,
// const bool updateKinematics = true)
// {
// return se3::centerOfMass(m_model, data, q, computeSubtreeComs, updateKinematics);
// return pinocchio::centerOfMass(m_model, data, q, computeSubtreeComs, updateKinematics);
// }
// const Vector3 & com(Data & data, const Vector & q, const Vector & v,
// const bool computeSubtreeComs = true,
// const bool updateKinematics = true)
// {
// return se3::centerOfMass(m_model, data, q, v, computeSubtreeComs, updateKinematics);
// return pinocchio::centerOfMass(m_model, data, q, v, computeSubtreeComs, updateKinematics);
// }
} // namespace robots
......
......@@ -24,7 +24,7 @@ namespace tsid
{
using namespace math;
using namespace trajectories;
using namespace se3;
using namespace pinocchio;
TaskComEquality::TaskComEquality(const std::string & name,
RobotWrapper & robot):
......
......@@ -24,7 +24,7 @@ namespace tsid
{
using namespace math;
using namespace trajectories;
using namespace se3;
using namespace pinocchio;
TaskJointPosture::TaskJointPosture(const std::string & name,
RobotWrapper & robot):
......
......@@ -25,7 +25,7 @@ namespace tsid
{
using namespace math;
using namespace trajectories;
using namespace se3;
using namespace pinocchio;
TaskSE3Equality::TaskSE3Equality(const std::string & name,
RobotWrapper & robot,
......
......@@ -53,7 +53,7 @@ BOOST_AUTO_TEST_CASE ( test_contact_6d )
string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
RobotWrapper robot(urdfFileName,
package_dirs,
se3::JointModelFreeFlyer(),
pinocchio::JointModelFreeFlyer(),
false);
BOOST_REQUIRE(robot.model().existFrame(frameName));
......@@ -77,12 +77,12 @@ BOOST_AUTO_TEST_CASE ( test_contact_6d )
BOOST_CHECK(contact.Kp().isApprox(Kp));
BOOST_CHECK(contact.Kd().isApprox(Kd));
Vector q = robot.model().neutralConfiguration;
Vector q = neutral(robot.model());
Vector v = Vector::Zero(robot.nv());
se3::Data data(robot.model());
pinocchio::Data data(robot.model());
robot.computeAllTerms(data, q, v);
se3::SE3 H_ref = robot.position(data, robot.model().getJointId(frameName));
pinocchio::SE3 H_ref = robot.position(data, robot.model().getJointId(frameName));
contact.setReference(H_ref);
double t = 0.0;
......
......@@ -33,7 +33,7 @@ BOOST_AUTO_TEST_CASE ( test_robot_wrapper )
{
using namespace std;
using namespace se3;
using namespace pinocchio;
const string romeo_model_path = TSID_SOURCE_DIR"/models/romeo";
......@@ -43,7 +43,7 @@ BOOST_AUTO_TEST_CASE ( test_robot_wrapper )
RobotWrapper robot(urdfFileName,
package_dirs,
se3::JointModelFreeFlyer(),
pinocchio::JointModelFreeFlyer(),
false);
const Model & model = robot.model();
......@@ -57,7 +57,7 @@ BOOST_AUTO_TEST_CASE ( test_robot_wrapper )
ub.head<3>().fill(10.);
ub.segment<4>(3).fill(1.);
Vector q = se3::randomConfiguration(model,lb,ub);
Vector q = pinocchio::randomConfiguration(model,lb,ub);
Vector v = Vector::Ones(robot.nv());
Data data(robot.model());
robot.computeAllTerms(data, q, v);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment