Commit 4d9808ba authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add task for joint velocity and acceleration bounds.

parent 9ee02ea9
......@@ -15,15 +15,61 @@
// <http://www.gnu.org/licenses/>.
//
#ifndef __invdyn_tasks_fwd_hpp__
#define __invdyn_tasks_fwd_hpp__
#ifndef __invdyn_task_joint_bounds_hpp__
#define __invdyn_task_joint_bounds_hpp__
#include <tsid/tasks/task-motion.hpp>
#include <tsid/math/constraint-bound.hpp>
namespace tsid
{
namespace tasks
{
class TaskJointBounds : public TaskMotion
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Vector Vector;
typedef math::ConstraintBound ConstraintBound;
typedef pinocchio::Data Data;
TaskJointBounds(const std::string & name,
RobotWrapper & robot,
double dt);
int dim() const;
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
const Data & data);
const ConstraintBase & getConstraint() const;
void setTimeStep(double dt);
void setVelocityBounds(ConstRefVector lower, ConstRefVector upper);
void setAccelerationBounds(ConstRefVector lower, ConstRefVector upper);
const Vector & getAccelerationLowerBounds() const;
const Vector & getAccelerationUpperBounds() const;
const Vector & getVelocityLowerBounds() const;
const Vector & getVelocityUpperBounds() const;
// const Vector & mask() const;
// void mask(const Vector & mask);
protected:
double m_dt;
int m_na, m_nv;
Vector m_v_lb, m_v_ub;
Vector m_a_lb, m_a_ub;
Vector m_ddq_max_due_to_vel, m_ddq_min_due_to_vel;
// Vector m_mask;
ConstraintBound m_constraint;
};
}
}
#endif // ifndef __invdyn_tasks_fwd_hpp__
#endif // ifndef __invdyn_task_joint_bounds_hpp__
......@@ -36,24 +36,25 @@ namespace tsid
TaskMotion(const std::string & name,
RobotWrapper & robot);
virtual const TrajectorySample & getReference() const = 0;
virtual const TrajectorySample & getReference() const;
virtual const Vector & getDesiredAcceleration() const = 0;
virtual const Vector & getDesiredAcceleration() const;
virtual Vector getAcceleration(ConstRefVector dv) const = 0;
virtual Vector getAcceleration(ConstRefVector dv) const;
virtual const Vector & position_error() const = 0;
virtual const Vector & velocity_error() const = 0;
virtual const Vector & position() const = 0;
virtual const Vector & velocity() const = 0;
virtual const Vector & position_ref() const = 0;
virtual const Vector & velocity_ref() const = 0;
virtual const Vector & position_error() const;
virtual const Vector & velocity_error() const;
virtual const Vector & position() const;
virtual const Vector & velocity() const;
virtual const Vector & position_ref() const;
virtual const Vector & velocity_ref() const;
virtual void setMask(math::ConstRefVector mask);
virtual bool hasMask();
protected:
math::Vector m_mask;
math::Vector m_dummy;
};
}
}
......
......@@ -119,8 +119,21 @@ unsigned int ContactPoint::n_force() const { return 3; }
const Vector & ContactPoint::Kp() const { return m_motionTask.Kp().head<3>(); }
const Vector & ContactPoint::Kd() const { return m_motionTask.Kd().head<3>(); }
void ContactPoint::Kp(ConstRefVector Kp){ Vector6 Kp6; Kp6.head<3>() = Kp; m_motionTask.Kp(Kp6); }
void ContactPoint::Kd(ConstRefVector Kd){ Vector6 Kd6; Kd6.head<3>() = Kd; m_motionTask.Kd(Kd6); }
void ContactPoint::Kp(ConstRefVector Kp)
{
assert(Kp.size()==3);
Vector6 Kp6;
Kp6.head<3>() = Kp;
m_motionTask.Kp(Kp6);
}
void ContactPoint::Kd(ConstRefVector Kd)
{
assert(Kd.size()==3);
Vector6 Kd6;
Kd6.head<3>() = Kd;
m_motionTask.Kd(Kd6);
}
bool ContactPoint::setContactNormal(ConstRefVector contactNormal)
{
......
......@@ -107,14 +107,15 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
if(priorityLevel==0)
m_eq += c.rows();
}
else if(c.isInequality())
else //if(c.isInequality())
{
tl->constraint = new ConstraintInequality(c.name(), c.rows(), m_v+m_k);
if(priorityLevel==0)
m_in += c.rows();
}
else
tl->constraint = new ConstraintBound(c.name(), m_v+m_k);
// don't use bounds for now because EiQuadProg doesn't exploit them anyway
// else
// tl->constraint = new ConstraintBound(c.name(), m_v+m_k);
m_hqpData[priorityLevel].push_back(make_pair<double, ConstraintBase*>(weight, tl->constraint));
}
......@@ -376,8 +377,9 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
}
else
{
(*it)->constraint->lowerBound().head(m_v) = c.lowerBound();
(*it)->constraint->upperBound().head(m_v) = c.upperBound();
(*it)->constraint->matrix().leftCols(m_v) = Matrix::Identity(m_v, m_v);
(*it)->constraint->lowerBound() = c.lowerBound();
(*it)->constraint->upperBound() = c.upperBound();
}
}
......
......@@ -15,14 +15,102 @@
// <http://www.gnu.org/licenses/>.
//
#include <Eigen/Dense>
#include <pinocchio/multibody/model.hpp>
#include <tsid/tasks/task-joint-bounds.hpp>
#include "tsid/robots/robot-wrapper.hpp"
namespace tsid
{
namespace tasks
{
using namespace math;
using namespace trajectories;
using namespace pinocchio;
TaskJointBounds::TaskJointBounds(const std::string & name,
RobotWrapper & robot,
double dt):
TaskMotion(name, robot),
m_constraint(name, robot.nv()),
m_dt(dt),
m_nv(robot.nv()),
m_na(robot.na())
{
assert(dt>0.0);
m_v_lb = -1e10*Vector::Ones(m_na);
m_v_ub = +1e10*Vector::Ones(m_na);
m_a_lb = -1e10*Vector::Ones(m_na);
m_a_ub = +1e10*Vector::Ones(m_na);
m_ddq_max_due_to_vel.setZero(m_na);
m_ddq_max_due_to_vel.setZero(m_na);
int offset = m_nv-m_na;
for(int i=0; i<offset; i++)
{
m_constraint.upperBound()(i) = 1e10;
m_constraint.lowerBound()(i) = -1e10;
}
}
int TaskJointBounds::dim() const
{ return m_nv; }
const Vector & TaskJointBounds::getAccelerationLowerBounds() const
{ return m_a_lb; }
const Vector & TaskJointBounds::getAccelerationUpperBounds() const
{ return m_a_ub; }
const Vector & TaskJointBounds::getVelocityLowerBounds() const
{ return m_v_lb; }
const Vector & TaskJointBounds::getVelocityUpperBounds() const
{ return m_v_ub; }
void TaskJointBounds::setTimeStep(double dt)
{
assert(dt>0);
m_dt = dt;
}
void TaskJointBounds::setVelocityBounds(ConstRefVector lower, ConstRefVector upper)
{
assert(lower.size()==m_na);
assert(upper.size()==m_na);
m_v_lb = lower;
m_v_ub = upper;
}
void TaskJointBounds::setAccelerationBounds(ConstRefVector lower, ConstRefVector upper)
{
assert(lower.size()==m_na);
assert(upper.size()==m_na);
m_a_lb = lower;
m_a_ub = upper;
}
const ConstraintBase & TaskJointBounds::getConstraint() const
{
return m_constraint;
}
const ConstraintBase & TaskJointBounds::compute(const double ,
ConstRefVector ,
ConstRefVector v,
const Data & )
{
// compute min/max joint acc imposed by velocity limits
m_ddq_max_due_to_vel = (m_v_ub - v.tail(m_na))/m_dt;
m_ddq_min_due_to_vel = (m_v_lb - v.tail(m_na))/m_dt;
// take most conservative limit between vel and acc
int offset = m_nv-m_na;
for(int i=0; i<m_na; i++)
{
m_constraint.upperBound()(offset+i) = std::min(m_ddq_max_due_to_vel(i), m_a_ub(i));
m_constraint.lowerBound()(offset+i) = std::max(m_ddq_min_due_to_vel(i), m_a_lb(i));
}
return m_constraint;
}
}
}
......@@ -21,6 +21,10 @@ namespace tsid
{
namespace tasks
{
typedef math::Vector Vector;
typedef trajectories::TrajectorySample TrajectorySample;
TaskMotion::TaskMotion(const std::string & name,
RobotWrapper & robot):
TaskBase(name, robot)
......@@ -35,5 +39,19 @@ namespace tsid
{
return m_mask.size() > 0;
}
const TrajectorySample & TaskMotion::getReference() const { return TrajectorySample(); }
const Vector & TaskMotion::getDesiredAcceleration() const { return m_dummy; }
Vector TaskMotion::getAcceleration(ConstRefVector ) const { return m_dummy; }
const Vector & TaskMotion::position_error() const { return m_dummy; }
const Vector & TaskMotion::velocity_error() const { return m_dummy; }
const Vector & TaskMotion::position() const { return m_dummy; }
const Vector & TaskMotion::velocity() const { return m_dummy; }
const Vector & TaskMotion::position_ref() const { return m_dummy; }
const Vector & TaskMotion::velocity_ref() const { return m_dummy; }
}
}
Markdown is supported
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