Commit e3271fff authored by andreadelprete's avatar andreadelprete
Browse files

Add contacts and inverse-dynamics-formulation classes with relative tests.

parent f3874b2b
......@@ -101,6 +101,13 @@ SET(${PROJECT_NAME}_TASKS_HEADERS
include/pininvdyn/tasks/task-actuation-equality.hpp
include/pininvdyn/tasks/task-actuation-bounds.hpp
include/pininvdyn/tasks/task-joint-bounds.hpp
include/pininvdyn/tasks/task-joint-posture.hpp
)
SET(${PROJECT_NAME}_CONTACTS_HEADERS
include/pininvdyn/contacts/fwd.hpp
include/pininvdyn/contacts/contact-base.hpp
include/pininvdyn/contacts/contact-6d.hpp
)
SET(${PROJECT_NAME}_TRAJECTORIES_HEADERS
......@@ -122,8 +129,10 @@ SET(HEADERS
include/pininvdyn/config.hpp
include/pininvdyn/robot-wrapper.hpp
include/pininvdyn/inverse-dynamics-formulation-base.hpp
include/pininvdyn/inverse-dynamics-formulation-acc-force.hpp
${${PROJECT_NAME}_MATH_HEADERS}
${${PROJECT_NAME}_TASKS_HEADERS}
${${PROJECT_NAME}_CONTACTS_HEADERS}
${${PROJECT_NAME}_TRAJECTORIES_HEADERS}
${${PROJECT_NAME}_SOLVERS_HEADERS}
)
......
//
// 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/>.
//
#ifndef __invdyn_contact_6d_hpp__
#define __invdyn_contact_6d_hpp__
#include <pininvdyn/contacts/contact-base.hpp>
#include <pininvdyn/tasks/task-se3-equality.hpp>
namespace pininvdyn
{
namespace contacts
{
class Contact6d:
public ContactBase
{
public:
typedef pininvdyn::RobotWrapper RobotWrapper;
typedef pininvdyn::math::ConstRefMatrix ConstRefMatrix;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::math::Matrix3x Matrix3x;
typedef pininvdyn::math::Vector3 Vector3;
typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::tasks::TaskMotion TaskMotion;
typedef pininvdyn::tasks::TaskSE3Equality TaskSE3Equality;
typedef pininvdyn::math::ConstraintInequality ConstraintInequality;
typedef pininvdyn::math::ConstraintEquality ConstraintEquality;
typedef se3::SE3 SE3;
Contact6d(const std::string & name,
RobotWrapper & robot,
const std::string & frameName,
ConstRefMatrix contactPoints,
ConstRefVector contactNormal,
const double frictionCoefficient,
const double minNormalForce,
const double regularizationTaskWeight);
/// Return the number of motion constraints
virtual unsigned int n_motion() const;
/// Return the number of force variables
virtual unsigned int n_force() const;
virtual const ConstraintBase & computeMotionTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
virtual const ConstraintInequality & computeForceTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
virtual const Matrix & getForceGeneratorMatrix();
virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const TaskMotion & getMotionTask() const;
const ConstraintBase & getMotionConstraint() const;
const ConstraintInequality & getForceConstraint() const;
const ConstraintEquality & getForceRegularizationTask() const;
double getForceRegularizationWeight() const;
const Vector & Kp() const;
const Vector & Kd() const;
void Kp(ConstRefVector Kp);
void Kd(ConstRefVector Kp);
bool setContactPoints(ConstRefMatrix contactPoints);
bool setContactNormal(ConstRefVector contactNormal);
bool setFrictionCoefficient(const double frictionCoefficient);
bool setMinNormalForce(const double minNormalForce);
bool setRegularizationTaskWeight(const double w);
void setReference(const SE3 & ref);
protected:
void updateForceInequalityConstraints();
void updateForceRegularizationTask();
void updateForceGeneratorMatrix();
TaskSE3Equality m_motionTask;
ConstraintInequality m_forceInequality;
ConstraintEquality m_forceRegTask;
Matrix3x m_contactPoints;
Vector3 m_contactNormal;
double m_mu;
double m_fMin;
double m_regularizationTaskWeight;
Matrix m_forceGenMat;
};
}
}
#endif // ifndef __invdyn_contact_6d_hpp__
//
// 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/>.
//
#ifndef __invdyn_contact_base_hpp__
#define __invdyn_contact_base_hpp__
#include <pininvdyn/robot-wrapper.hpp>
#include <pininvdyn/math/utils.hpp>
#include <pininvdyn/tasks/task-motion.hpp>
#include <pininvdyn/math/constraint-base.hpp>
#include <pininvdyn/math/constraint-equality.hpp>
#include <pininvdyn/math/constraint-inequality.hpp>
namespace pininvdyn
{
namespace contacts
{
///
/// \brief Base template of a Contact.
///
class ContactBase
{
public:
typedef pininvdyn::RobotWrapper RobotWrapper;
typedef pininvdyn::math::ConstraintBase ConstraintBase;
typedef pininvdyn::math::ConstraintInequality ConstraintInequality;
typedef pininvdyn::math::ConstraintEquality ConstraintEquality;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::math::Matrix Matrix;
typedef pininvdyn::tasks::TaskMotion TaskMotion;
typedef se3::Data Data;
ContactBase(const std::string & name,
RobotWrapper & robot);
const std::string & name() const;
void name(const std::string & name);
/// Return the number of motion constraints
virtual unsigned int n_motion() const = 0;
/// Return the number of force variables
virtual unsigned int n_force() const = 0;
virtual const ConstraintBase & computeMotionTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data) = 0;
virtual const ConstraintInequality & computeForceTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data) = 0;
virtual const Matrix & getForceGeneratorMatrix() = 0;
virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data) = 0;
virtual const TaskMotion & getMotionTask() const = 0;
virtual const ConstraintBase & getMotionConstraint() const = 0;
virtual const ConstraintInequality & getForceConstraint() const = 0;
virtual const ConstraintEquality & getForceRegularizationTask() const = 0;
virtual double getForceRegularizationWeight() const = 0;
protected:
std::string m_name;
/// \brief Reference on the robot model.
RobotWrapper & m_robot;
};
}
}
#endif // ifndef __invdyn_contact_base_hpp__
//
// 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_tasks_fwd_hpp__
#define __invdyn_tasks_fwd_hpp__
namespace pininvdyn
{
namespace tasks
{
}
}
#endif // ifndef __invdyn_tasks_fwd_hpp__
//
// 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/>.
//
#ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__
#define __invdyn_inverse_dynamics_formulation_acc_force_hpp__
#include <pininvdyn/inverse-dynamics-formulation-base.hpp>
#include <vector>
namespace pininvdyn
{
class TaskLevel
{
public:
pininvdyn::tasks::TaskBase & task;
pininvdyn::math::ConstraintBase * constraint;
double weight;
unsigned int priority;
TaskLevel(pininvdyn::tasks::TaskBase & task,
double weight,
unsigned int priority);
};
class ContactLevel
{
public:
pininvdyn::contacts::ContactBase & contact;
pininvdyn::math::ConstraintBase * motionConstraint;
pininvdyn::math::ConstraintInequality * forceConstraint;
pininvdyn::math::ConstraintEquality * forceRegTask;
unsigned int index; /// index of 1st element of associated force variable in the force vector
ContactLevel(pininvdyn::contacts::ContactBase & contact);
};
class InverseDynamicsFormulationAccForce:
public InverseDynamicsFormulationBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef se3::Data Data;
typedef pininvdyn::math::Matrix Matrix;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::tasks::TaskBase TaskBase;
typedef pininvdyn::tasks::TaskMotion TaskMotion;
typedef pininvdyn::tasks::TaskContactForce TaskContactForce;
typedef pininvdyn::tasks::TaskActuation TaskActuation;
InverseDynamicsFormulationAccForce(const std::string & name,
RobotWrapper & robot,
bool verbose=false);
const Data & data() const;
unsigned int nVar() const;
unsigned int nEq() const;
unsigned int nIn() const;
bool addMotionTask(TaskMotion & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0);
bool addForceTask(TaskContactForce & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0);
bool addTorqueTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0);
bool addRigidContact(ContactBase & contact);
bool removeTask(const std::string & taskName,
double transition_duration=0.0);
bool removeRigidContact(const std::string & contactName,
double transition_duration=0.0);
const HqpData & computeProblemData(double time,
ConstRefVector q,
ConstRefVector v);
public:
void addTask(TaskLevel* task,
double weight,
unsigned int priorityLevel);
void resizeHqpData();
Data m_data;
HqpData m_hqpData;
std::vector<TaskLevel*> m_taskMotions;
std::vector<TaskLevel*> m_taskContactForces;
std::vector<TaskLevel*> m_taskActuations;
std::vector<ContactLevel*> m_contacts;
double m_t; /// time
unsigned int m_k; /// number of contact-force variables
unsigned int m_v; /// number of acceleration variables
unsigned int m_eq; /// number of equality constraints
unsigned int m_in; /// number of inequality constraints
Matrix m_Jc; /// contact force Jacobian
pininvdyn::math::ConstraintEquality m_baseDynamics;
};
}
#endif // ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__
//
// 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/>.
//
#ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
#define __invdyn_inverse_dynamics_formulation_base_hpp__
#include <pininvdyn/math/utils.hpp>
#include <pininvdyn/robot-wrapper.hpp>
#include <pininvdyn/tasks/task-actuation.hpp>
#include <pininvdyn/tasks/task-motion.hpp>
#include <pininvdyn/tasks/task-contact-force.hpp>
#include <pininvdyn/contacts/contact-base.hpp>
#include <pininvdyn/solvers/solver-HQP-base.hpp>
#include <string>
namespace pininvdyn
{
///
/// \brief Wrapper for a robot based on pinocchio
///
class InverseDynamicsFormulationBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef se3::Data Data;
typedef pininvdyn::math::ConstRefVector ConstRefVector;
typedef pininvdyn::tasks::TaskMotion TaskMotion;
typedef pininvdyn::tasks::TaskContactForce TaskContactForce;
typedef pininvdyn::tasks::TaskActuation TaskActuation;
typedef pininvdyn::contacts::ContactBase ContactBase;
typedef pininvdyn::solvers::HqpData HqpData;
InverseDynamicsFormulationBase(const std::string & name,
RobotWrapper & robot,
bool verbose=false);
virtual const Data & data() const = 0;
virtual unsigned int nVar() const = 0;
virtual unsigned int nEq() const = 0;
virtual unsigned int nIn() const = 0;
virtual bool addMotionTask(TaskMotion & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0) = 0;
virtual bool addForceTask(TaskContactForce & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0) = 0;
virtual bool addTorqueTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0) = 0;
virtual bool addRigidContact(ContactBase & contact) = 0;
virtual bool removeTask(const std::string & taskName,
double transition_duration=0.0) = 0;
virtual bool removeRigidContact(const std::string & contactName,
double transition_duration=0.0) = 0;
virtual const HqpData & computeProblemData(double time,
ConstRefVector q,
ConstRefVector v) = 0;
protected:
std::string m_name;
RobotWrapper m_robot;
bool m_verbose;
};
}
#endif // ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
......@@ -39,37 +39,41 @@ namespace pininvdyn
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ConstraintBase(std::string name):
m_name(name){}
ConstraintBase(const std::string & name);
ConstraintBase(std::string name, const unsigned int rows, const unsigned int cols):
m_name(name)
{
m_A = Matrix::Zero(rows, cols);
}
ConstraintBase(const std::string & name,
const unsigned int rows,
const unsigned int cols);
ConstraintBase(std::string name, const Matrix & A):
m_name(name),
m_A(A)
{}
ConstraintBase(const std::string & name,
ConstRefMatrix A);
virtual const std::string & name() const;
virtual unsigned int rows() const = 0;
virtual unsigned int cols() const = 0;
virtual void resize(const unsigned int r, const unsigned int c) = 0;
virtual bool isEquality() const = 0;
virtual bool isInequality() const = 0;
virtual bool isBound() const = 0;
virtual const Matrix & matrix() const { return m_A; }
virtual const Matrix & matrix() const;
virtual const Vector & vector() const = 0;
virtual const Vector & lowerBound() const = 0;
virtual const Vector & upperBound() const = 0;
virtual bool setMatrix(ConstRefMatrix A) { m_A = A; return true; }
virtual Matrix & matrix();
virtual Vector & vector() = 0;
virtual Vector & lowerBound() = 0;
virtual Vector & upperBound() = 0;
virtual bool setMatrix(ConstRefMatrix A);
virtual bool setVector(ConstRefVector b) = 0;
virtual bool setLowerBound(ConstRefVector lb) = 0;
virtual bool setUpperBound(ConstRefVector ub) = 0;
virtual bool checkConstraint(ConstRefVector x, double tol=1e-6) const = 0;
protected:
std::string m_name;
Matrix m_A;
......
......@@ -2,16 +2,16 @@
// Copyright (c) 2017 CNRS
//
// This file is part of PinInvDyn
// pinocchio is free software: you can redistribute it
// 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.
// pinocchio is distributed in the hope that it will be
// 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
// pinocchio If not, see
// PinInvDyn If not, see
// <http://www.gnu.org/licenses/>.
//
......@@ -32,45 +32,36 @@ namespace pininvdyn
{
public:
ConstraintBound(std::string name):
ConstraintBase(name)
{}
ConstraintBound(std::string name, const unsigned int size):
ConstraintBase(name),
m_lb(Vector::Zero(size)),
m_ub(Vector::Zero(size))
{}
ConstraintBound(std::string name, const Vector & lb, const Vector & ub):
ConstraintBase(name),
m_lb(lb),
m_ub(ub)
{}
inline unsigned int rows() const
{
assert(m_lb.rows()==m_ub.rows());
return (unsigned int) m_lb.rows();
}
inline unsigned int cols() const
{
assert(m_lb.rows()==m_ub.rows());
return (unsigned int) m_lb.rows();
}
inline bool isEquality() const { return false; }