Commit 4f84e1e0 authored by andreadelprete's avatar andreadelprete
Browse files

Add com-equality-task. Add unit tests for com-equality and se3 tasks. Add...

Add com-equality-task. Add unit tests for com-equality and se3 tasks. Add functions to compute pseudo-inverses.
parent 0d74fa3b
......@@ -95,6 +95,7 @@ SET(${PROJECT_NAME}_TASKS_HEADERS
include/pininvdyn/tasks/task-motion.hpp
include/pininvdyn/tasks/task-actuation.hpp
include/pininvdyn/tasks/task-contact-force.hpp
include/pininvdyn/tasks/task-com-equality.hpp
include/pininvdyn/tasks/task-se3-equality.hpp
include/pininvdyn/tasks/task-contact-force-equality.hpp
include/pininvdyn/tasks/task-actuation-equality.hpp
......
......@@ -42,36 +42,60 @@ namespace pininvdyn
typedef std::size_t Index;
/**
* Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,Q1,Q2,Q3,Q4].
* Convert the input SE3 object to a 7D vector of floats [X,Y,Z,Q1,Q2,Q3,Q4].
*/
static void se3ToXYZQUAT(const se3::SE3 & M, RefVector xyzQuat)
{
assert(xyzQuat.size()>=7);
xyzQuat.head<3>() = M.translation();
xyzQuat.tail<4>() = Eigen::Quaterniond(M.rotation()).coeffs();
}
void se3ToXYZQUAT(const se3::SE3 & M, RefVector xyzQuat);
static void se3ToVector(const se3::SE3 & M, RefVector vec)
{
assert(vec.size()>=12);
vec.head<3>() = M.translation();
typedef Eigen::Matrix<double,9,1> Vector9;
vec.tail<9>() = Eigen::Map<const Vector9>(&M.rotation()(0), 9);
}
/**
* 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);
static void vectorToSE3(RefVector vec, se3::SE3 & M)
{
assert(vec.size()>=12);
M.translation( vec.head<3>() );
typedef Eigen::Matrix<double,3,3> Matrix3;
M.rotation( Eigen::Map<const Matrix3>(&vec(3), 3, 3) );
}
void vectorToSE3(RefVector vec, se3::SE3 & M);
void errorInSE3 (const se3::SE3 & M,
const se3::SE3 & Mdes,
se3::Motion & error);
void pseudoInverse(ConstRefMatrix A,
RefMatrix Apinv,
double tolerance,
unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV);
void pseudoInverse(ConstRefMatrix A,
Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
RefMatrix Apinv,
double tolerance,
unsigned int computationOptions);
void pseudoInverse(ConstRefMatrix A,
Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
RefMatrix Apinv,
double tolerance,
double * nullSpaceBasisOfA,
int &nullSpaceRows,
int &nullSpaceCols,
unsigned int computationOptions);
void dampedPseudoInverse(ConstRefMatrix A,
Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
RefMatrix Apinv,
double tolerance,
double dampingFactor,
unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV,
double * nullSpaceBasisOfA=0,
int *nullSpaceRows=0, int *nullSpaceCols=0);
void nullSpaceBasisFromDecomposition(Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
double tolerance,
double * nullSpaceBasisMatrix,
int &rows, int &cols);
static void errorInSE3 (const se3::SE3 & M,
const se3::SE3 & Mdes,
se3::Motion & error)
template<typename Derived>
inline bool is_finite(const Eigen::MatrixBase<Derived>& x)
{
error = se3::log6(Mdes.inverse() * M);
return ( (x - x).array() == (x - x).array()).all();
}
}
......
......@@ -25,6 +25,7 @@
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/spatial/skew.hpp"
#include <pininvdyn/math/utils.hpp>
#include <string>
......@@ -50,6 +51,7 @@ namespace pininvdyn
typedef Eigen::Matrix<Scalar,6,1> Vector6;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
typedef pininvdyn::math::RefVector RefVector;
RobotWrapper(const std::string & filename,
......@@ -73,6 +75,11 @@ namespace pininvdyn
void computeAllTerms(Data & data, const Vector & q, const Vector & v) const;
const void com(const Data & data,
RefVector com_pos,
RefVector com_vel,
RefVector com_acc) const;
const Vector3 & com(const Data & data) const;
const Vector3 & com_vel(const Data & data) const;
......
//
// Copyright (c) 2017 CNRS
//
// This file is part of PinInvDyn
// 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 __invdyn_task_com_equality_hpp__
#define __invdyn_task_com_equality_hpp__
#include <pininvdyn/tasks/task-motion.hpp>
#include <pininvdyn/trajectories/trajectory-base.hpp>
#include <pininvdyn/math/constraint-equality.hpp>
namespace pininvdyn
{
namespace tasks
{
class TaskComEquality:
public TaskMotion
{
public:
typedef pininvdyn::RobotWrapper RobotWrapper;
typedef pininvdyn::math::Index Index;
typedef pininvdyn::trajectories::TrajectorySample TrajectorySample;
typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::math::Vector3 Vector3;
typedef pininvdyn::math::ConstraintEquality ConstraintEquality;
typedef se3::Data Data;
typedef se3::Data::Matrix6x Matrix6x;
typedef se3::Motion Motion;
typedef se3::SE3 SE3;
TaskComEquality(const std::string & name,
RobotWrapper & robot);
int dim() const;
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
void setReference(const TrajectorySample & ref);
const Vector3 & position_error() const;
const Vector3 & velocity_error() const;
const Vector3 & Kp();
const Vector3 & Kd();
void Kp(ConstRefVector Kp);
void Kd(ConstRefVector Kp);
protected:
Vector3 m_Kp;
Vector3 m_Kd;
Vector3 m_p_error, m_v_error;
TrajectorySample m_ref;
ConstraintEquality m_constraint;
};
}
}
#endif // ifndef __invdyn_task_com_equality_hpp__
......@@ -54,6 +54,9 @@ namespace pininvdyn
void setReference(TrajectorySample & ref);
const Motion & position_error() const;
const Motion & velocity_error() const;
const Vector & Kp();
const Vector & Kd();
void Kp(ConstRefVector Kp);
......
......@@ -18,6 +18,7 @@
SET(LIBRARY_NAME ${PROJECT_NAME})
SET(${LIBRARY_NAME}_MATH_SOURCES
math/utils.cpp
)
SET(${LIBRARY_NAME}_TASKS_SOURCES
......@@ -25,6 +26,7 @@ SET(${LIBRARY_NAME}_TASKS_SOURCES
tasks/task-actuation-bounds.cpp
tasks/task-actuation-equality.cpp
tasks/task-actuation.cpp
tasks/task-com-equality.cpp
tasks/task-contact-force-equality.cpp
tasks/task-contact-force.cpp
tasks/task-joint-bounds.cpp
......
//
// Copyright (c) 2017 CNRS
//
// This file is part of PinInvDyn
// PinInvDyn 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.
// PinInvDyn 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
// PinInvDyn If not, see
// <http://www.gnu.org/licenses/>.
//
#include <pininvdyn/math/utils.hpp>
void pininvdyn::math::se3ToXYZQUAT(const se3::SE3 & M, RefVector xyzQuat)
{
assert(xyzQuat.size()>=7);
xyzQuat.head<3>() = M.translation();
xyzQuat.tail<4>() = Eigen::Quaterniond(M.rotation()).coeffs();
}
void pininvdyn::math::se3ToVector(const se3::SE3 & M, RefVector vec)
{
assert(vec.size()>=12);
vec.head<3>() = M.translation();
typedef Eigen::Matrix<double,9,1> Vector9;
vec.tail<9>() = Eigen::Map<const Vector9>(&M.rotation()(0), 9);
}
void pininvdyn::math::vectorToSE3(RefVector vec, se3::SE3 & M)
{
assert(vec.size()>=12);
M.translation( vec.head<3>() );
typedef Eigen::Matrix<double,3,3> Matrix3;
M.rotation( Eigen::Map<const Matrix3>(&vec(3), 3, 3) );
}
void pininvdyn::math::errorInSE3 (const se3::SE3 & M,
const se3::SE3 & Mdes,
se3::Motion & error)
{
error = se3::log6(Mdes.inverse() * M);
}
void pininvdyn::math::pseudoInverse(ConstRefMatrix A,
RefMatrix Apinv,
double tolerance,
unsigned int computationOptions)
{
Eigen::JacobiSVD<typename Eigen::MatrixXd::PlainObject> svdDecomposition(A.rows(), A.cols());
pininvdyn::math::pseudoInverse(A, svdDecomposition, Apinv, tolerance, computationOptions);
}
void pininvdyn::math::pseudoInverse(ConstRefMatrix A,
Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
RefMatrix Apinv,
double tolerance,
unsigned int computationOptions)
{
using namespace Eigen;
int nullSpaceRows = -1, nullSpaceCols = -1;
pseudoInverse(A, svdDecomposition, Apinv, tolerance,
(double*)0, nullSpaceRows, nullSpaceCols, computationOptions);
}
void pininvdyn::math::pseudoInverse(ConstRefMatrix A,
Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
RefMatrix Apinv,
double tolerance,
double * nullSpaceBasisOfA,
int &nullSpaceRows, int &nullSpaceCols,
unsigned int computationOptions)
{
using namespace Eigen;
if (computationOptions == 0) return; //if no computation options we cannot compute the pseudo inverse
svdDecomposition.compute(A, computationOptions);
JacobiSVD<MatrixXd::PlainObject>::SingularValuesType singularValues = svdDecomposition.singularValues();
int singularValuesSize = singularValues.size();
int rank = 0;
for (int idx = 0; idx < singularValuesSize; idx++) {
if (tolerance > 0 && singularValues(idx) > tolerance) {
singularValues(idx) = 1.0 / singularValues(idx);
rank++;
} else {
singularValues(idx) = 0.0;
}
}
//equivalent to this U/V matrix in case they are computed full
Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * singularValues.asDiagonal() * svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint();
if (nullSpaceBasisOfA && (computationOptions & ComputeFullV)) {
//we can compute the null space basis for A
nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisOfA, nullSpaceRows, nullSpaceCols);
}
}
void pininvdyn::math::dampedPseudoInverse(ConstRefMatrix A,
Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
RefMatrix Apinv,
double tolerance,
double dampingFactor,
unsigned int computationOptions,
double * nullSpaceBasisOfA,
int *nullSpaceRows, int *nullSpaceCols)
{
using namespace Eigen;
if (computationOptions == 0) return; //if no computation options we cannot compute the pseudo inverse
svdDecomposition.compute(A, computationOptions);
JacobiSVD<MatrixXd::PlainObject>::SingularValuesType singularValues = svdDecomposition.singularValues();
//rank will be used for the null space basis.
//not sure if this is correct
const int singularValuesSize = singularValues.size();
const double d2 = dampingFactor * dampingFactor;
int rank = 0;
for (int idx = 0; idx < singularValuesSize; idx++)
{
if(singularValues(idx) > tolerance)
rank++;
singularValues(idx) = singularValues(idx) / ((singularValues(idx) * singularValues(idx)) + d2);
}
//equivalent to this U/V matrix in case they are computed full
Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * singularValues.asDiagonal() * svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint();
if (nullSpaceBasisOfA && nullSpaceRows && nullSpaceCols
&& (computationOptions & ComputeFullV))
{
//we can compute the null space basis for A
nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisOfA, *nullSpaceRows, *nullSpaceCols);
}
}
void pininvdyn::math::nullSpaceBasisFromDecomposition(Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
double tolerance,
double * nullSpaceBasisMatrix,
int &rows, int &cols)
{
using namespace Eigen;
JacobiSVD<MatrixXd::PlainObject>::SingularValuesType singularValues = svdDecomposition.singularValues();
int rank = 0;
for (int idx = 0; idx < singularValues.size(); idx++) {
if (tolerance > 0 && singularValues(idx) > tolerance) {
rank++;
}
}
nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisMatrix, rows, cols);
}
void nullSpaceBasisFromDecomposition(Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
int rank,
double * nullSpaceBasisMatrix,
int &rows, int &cols)
{
using namespace Eigen;
const MatrixXd &vMatrix = svdDecomposition.matrixV();
//A \in \mathbb{R}^{uMatrix.rows() \times vMatrix.cols()}
rows = vMatrix.cols();
cols = vMatrix.cols() - rank;
Map<MatrixXd> map(nullSpaceBasisMatrix, rows, cols);
map = vMatrix.rightCols(vMatrix.cols() - rank);
}
......@@ -54,6 +54,16 @@ namespace pininvdyn
se3::framesForwardKinematics(m_model, data);
}
const void RobotWrapper::com(const Data & data,
RefVector com_pos,
RefVector com_vel,
RefVector com_acc) const
{
com_pos = data.com[0];
com_vel = data.vcom[0];
com_acc = data.acom[0];
}
const Vector3 & RobotWrapper::com(const Data & data) const
{
return data.com[0];
......
//
// Copyright (c) 2017 CNRS
//
// This file is part of PinInvDyn
// PinInvDyn 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.
// PinInvDyn 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
// PinInvDyn If not, see
// <http://www.gnu.org/licenses/>.
//
#include <pininvdyn/tasks/task-com-equality.hpp>
namespace pininvdyn
{
namespace tasks
{
using namespace pininvdyn::math;
using namespace se3;
TaskComEquality::TaskComEquality(const std::string & name,
RobotWrapper & robot):
TaskMotion(name, robot),
m_constraint(name, 3, robot.nv())
{
m_Kp.setZero(3);
m_Kd.setZero(3);
}
int TaskComEquality::dim() const
{
//return self._mask.sum ()
return 3;
}
const Vector3 & TaskComEquality::Kp(){ return m_Kp; }
const Vector3 & TaskComEquality::Kd(){ return m_Kd; }
void TaskComEquality::Kp(ConstRefVector Kp)
{
assert(Kp.size()==3);
m_Kp = Kp;
}
void TaskComEquality::Kd(ConstRefVector Kd)
{
assert(Kd.size()==3);
m_Kd = Kd;
}
void TaskComEquality::setReference(const TrajectorySample & ref)
{
m_ref = ref;
}
const Vector3 & TaskComEquality::position_error() const
{
return m_p_error;
}
const Vector3 & TaskComEquality::velocity_error() const
{
return m_v_error;
}
const ConstraintBase & TaskComEquality::compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data)
{
Vector3 p_com, v_com, drift;
m_robot.com(data, p_com, v_com, drift);
// Compute errors
m_p_error = p_com - m_ref.pos;
m_v_error = v_com - m_ref.vel;
Vector3 m_a_des = - m_Kp.cwiseProduct(m_p_error)
- m_Kd.cwiseProduct(m_v_error)
+ m_ref.acc;
// Get CoM jacobian
const Matrix3x & Jcom = m_robot.Jcom(data);
m_constraint.setMatrix(Jcom);
m_constraint.setVector(m_a_des - drift);
return m_constraint;
}
}
}
......@@ -73,6 +73,16 @@ namespace pininvdyn
m_a_ref = Motion(ref.acc);
}
const Motion & TaskSE3Equality::position_error() const
{
return m_p_error;
}
const Motion & TaskSE3Equality::velocity_error() const
{
return m_v_error;
}
const ConstraintBase & TaskSE3Equality::compute(const double t,
ConstRefVector q,
ConstRefVector v,
......@@ -95,15 +105,10 @@ namespace pininvdyn
- m_Kd.cwiseProduct(m_v_error.toVector_impl())
+ m_wMl.actInv(m_a_ref).toVector_impl();
// @todo Since Jacobian computation is cheaper in world frame
// we could do all computations in world frame
m_robot.frameJacobianLocal(data, m_frame_id, m_J);
// if(local_frame==False):
// drift = self._gMl.act(drift);
// a_des[:3] = self._gMl.rotation * a_des[:3];
// a_des[3:] = self._gMl.rotation * a_des[3:];
// J[:3,:] = self._gMl.rotation * J[:3,:];
// J[3:,:] = self._gMl.rotation * J[3:,:];
m_constraint.setMatrix(m_J);
m_constraint.setVector(m_a_des - drift.toVector_impl());
return m_constraint;
......
......@@ -54,4 +54,4 @@ ADD_UNIT_TEST(constraints eigen3)
ADD_UNIT_TEST(trajectories eigen3)
ADD_UNIT_TEST(robot-wrapper eigen3)
ADD_UNIT_TEST(tasks eigen3)
ADD_UNIT_TEST(math_utils eigen3)
//
// Copyright (c) 2017 CNRS
//
// This file is part of PinInvDyn
// PinInvDyn 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.
// PinInvDyn 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
// PinInvDyn If not, see
// <http://www.gnu.org/licenses/>.
//
#include <iostream>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <pininvdyn/math/utils.hpp>
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( test_pseudoinverse)
{
std::cout << "test_pseudoinverse\n";
using namespace pininvdyn::math;
using namespace Eigen;
using namespace std;
const unsigned int m = 3;
const unsigned int n = 5;
MatrixXd A = MatrixXd::Random(m,n);
MatrixXd Apinv = MatrixXd::Zero(n,m);
pseudoInverse(A, Apinv, 1e-5);
BOOST_CHECK(MatrixXd::Identity(m,m).isApprox(A*Apinv));
}
BOOST_AUTO_TEST_SUITE_END ()