Commit 83889f96 authored by andreadelprete's avatar andreadelprete
Browse files

Minor improvement everywhere

parent 7c7038e1
...@@ -60,19 +60,19 @@ namespace pininvdyn ...@@ -60,19 +60,19 @@ namespace pininvdyn
virtual const ConstraintBase & computeMotionTask(const double t, virtual const ConstraintBase & computeMotionTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data); const Data & data);
virtual const ConstraintInequality & computeForceTask(const double t, virtual const ConstraintInequality & computeForceTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data); const Data & data);
virtual const Matrix & getForceGeneratorMatrix(); virtual const Matrix & getForceGeneratorMatrix();
virtual const ConstraintEquality & computeForceRegularizationTask(const double t, virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data); const Data & data);
const TaskMotion & getMotionTask() const; const TaskMotion & getMotionTask() const;
const ConstraintBase & getMotionConstraint() const; const ConstraintBase & getMotionConstraint() const;
......
...@@ -61,19 +61,19 @@ namespace pininvdyn ...@@ -61,19 +61,19 @@ namespace pininvdyn
virtual const ConstraintBase & computeMotionTask(const double t, virtual const ConstraintBase & computeMotionTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) = 0; const Data & data) = 0;
virtual const ConstraintInequality & computeForceTask(const double t, virtual const ConstraintInequality & computeForceTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) = 0; const Data & data) = 0;
virtual const Matrix & getForceGeneratorMatrix() = 0; virtual const Matrix & getForceGeneratorMatrix() = 0;
virtual const ConstraintEquality & computeForceRegularizationTask(const double t, virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) = 0; const Data & data) = 0;
virtual const TaskMotion & getMotionTask() const = 0; virtual const TaskMotion & getMotionTask() const = 0;
virtual const ConstraintBase & getMotionConstraint() const = 0; virtual const ConstraintBase & getMotionConstraint() const = 0;
......
...@@ -53,7 +53,7 @@ namespace pininvdyn ...@@ -53,7 +53,7 @@ namespace pininvdyn
virtual const ConstraintBase & compute(const double t, virtual const ConstraintBase & compute(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) = 0; const Data & data) = 0;
virtual const ConstraintBase & getConstraint() const = 0; virtual const ConstraintBase & getConstraint() const = 0;
......
...@@ -50,11 +50,15 @@ namespace pininvdyn ...@@ -50,11 +50,15 @@ namespace pininvdyn
const ConstraintBase & compute(const double t, const ConstraintBase & compute(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data); const Data & data);
const ConstraintBase & getConstraint() const; const ConstraintBase & getConstraint() const;
void setReference(const TrajectorySample & ref); void setReference(const TrajectorySample & ref);
const TrajectorySample & getReference() const;
const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const;
const Vector & position_error() const; const Vector & position_error() const;
const Vector & velocity_error() const; const Vector & velocity_error() const;
...@@ -72,6 +76,8 @@ namespace pininvdyn ...@@ -72,6 +76,8 @@ namespace pininvdyn
Vector3 m_Kp; Vector3 m_Kp;
Vector3 m_Kd; Vector3 m_Kd;
Vector3 m_p_error, m_v_error; Vector3 m_p_error, m_v_error;
Vector3 m_a_des;
Vector3 m_drift;
Vector m_p_error_vec, m_v_error_vec; Vector m_p_error_vec, m_v_error_vec;
TrajectorySample m_ref; TrajectorySample m_ref;
ConstraintEquality m_constraint; ConstraintEquality m_constraint;
......
...@@ -47,11 +47,15 @@ namespace pininvdyn ...@@ -47,11 +47,15 @@ namespace pininvdyn
const ConstraintBase & compute(const double t, const ConstraintBase & compute(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data); const Data & data);
const ConstraintBase & getConstraint() const; const ConstraintBase & getConstraint() const;
void setReference(const TrajectorySample & ref); void setReference(const TrajectorySample & ref);
const TrajectorySample & getReference() const;
const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const;
const Vector & mask() const; const Vector & mask() const;
bool mask(const Vector & mask); bool mask(const Vector & mask);
...@@ -73,6 +77,7 @@ namespace pininvdyn ...@@ -73,6 +77,7 @@ namespace pininvdyn
Vector m_Kd; Vector m_Kd;
Vector m_p_error, m_v_error; Vector m_p_error, m_v_error;
Vector m_p, m_v; Vector m_p, m_v;
Vector m_a_des;
Vector m_mask; Vector m_mask;
VectorXi m_activeAxes; VectorXi m_activeAxes;
TrajectorySample m_ref; TrajectorySample m_ref;
......
...@@ -19,6 +19,7 @@ ...@@ -19,6 +19,7 @@
#define __invdyn_task_motion_hpp__ #define __invdyn_task_motion_hpp__
#include <pininvdyn/tasks/task-base.hpp> #include <pininvdyn/tasks/task-base.hpp>
#include <pininvdyn/trajectories/trajectory-base.hpp>
namespace pininvdyn namespace pininvdyn
{ {
...@@ -30,16 +31,24 @@ namespace pininvdyn ...@@ -30,16 +31,24 @@ namespace pininvdyn
public: public:
typedef pininvdyn::RobotWrapper RobotWrapper; typedef pininvdyn::RobotWrapper RobotWrapper;
typedef pininvdyn::math::Vector Vector; typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::trajectories::TrajectorySample TrajectorySample;
TaskMotion(const std::string & name, TaskMotion(const std::string & name,
RobotWrapper & robot); RobotWrapper & robot);
virtual const TrajectorySample & getReference() const = 0;
virtual const Vector & getDesiredAcceleration() const = 0;
virtual Vector getAcceleration(ConstRefVector dv) const = 0;
virtual const Vector & position_error() const = 0; virtual const Vector & position_error() const = 0;
virtual const Vector & velocity_error() const = 0; virtual const Vector & velocity_error() const = 0;
virtual const Vector & position() const = 0; virtual const Vector & position() const = 0;
virtual const Vector & velocity() const = 0; virtual const Vector & velocity() const = 0;
virtual const Vector & position_ref() const = 0; virtual const Vector & position_ref() const = 0;
virtual const Vector & velocity_ref() const = 0; virtual const Vector & velocity_ref() const = 0;
}; };
} }
} }
......
...@@ -50,11 +50,15 @@ namespace pininvdyn ...@@ -50,11 +50,15 @@ namespace pininvdyn
const ConstraintBase & compute(const double t, const ConstraintBase & compute(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data); const Data & data);
const ConstraintBase & getConstraint() const; const ConstraintBase & getConstraint() const;
void setReference(TrajectorySample & ref); void setReference(TrajectorySample & ref);
const TrajectorySample & getReference() const;
const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const;
const Vector & position_error() const; const Vector & position_error() const;
const Vector & velocity_error() const; const Vector & velocity_error() const;
...@@ -80,8 +84,10 @@ namespace pininvdyn ...@@ -80,8 +84,10 @@ namespace pininvdyn
Vector m_Kp; Vector m_Kp;
Vector m_Kd; Vector m_Kd;
Vector m_a_des; Vector m_a_des;
Motion m_drift;
Matrix6x m_J; Matrix6x m_J;
ConstraintEquality m_constraint; ConstraintEquality m_constraint;
TrajectorySample m_ref;
}; };
} }
......
...@@ -198,7 +198,7 @@ void Contact6d::setReference(const SE3 & ref) ...@@ -198,7 +198,7 @@ void Contact6d::setReference(const SE3 & ref)
const ConstraintBase & Contact6d::computeMotionTask(const double t, const ConstraintBase & Contact6d::computeMotionTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) const Data & data)
{ {
return m_motionTask.compute(t, q, v, data); return m_motionTask.compute(t, q, v, data);
} }
...@@ -206,7 +206,7 @@ const ConstraintBase & Contact6d::computeMotionTask(const double t, ...@@ -206,7 +206,7 @@ const ConstraintBase & Contact6d::computeMotionTask(const double t,
const ConstraintInequality & Contact6d::computeForceTask(const double t, const ConstraintInequality & Contact6d::computeForceTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) const Data & data)
{ {
return m_forceInequality; return m_forceInequality;
} }
...@@ -219,7 +219,7 @@ const Matrix & Contact6d::getForceGeneratorMatrix() ...@@ -219,7 +219,7 @@ const Matrix & Contact6d::getForceGeneratorMatrix()
const ConstraintEquality & Contact6d::computeForceRegularizationTask(const double t, const ConstraintEquality & Contact6d::computeForceRegularizationTask(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) const Data & data)
{ {
return m_forceRegTask; return m_forceRegTask;
} }
......
...@@ -221,7 +221,7 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti ...@@ -221,7 +221,7 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
{ {
const ContactTransitionInfo * c = *it_ct; const ContactTransitionInfo * c = *it_ct;
assert(c->time_start <= m_t); assert(c->time_start <= m_t);
if(m_t < c->time_end) if(m_t <= c->time_end)
{ {
const double alpha = (m_t - c->time_start) / (c->time_end - c->time_start); const double alpha = (m_t - c->time_start) / (c->time_end - c->time_start);
const double fMax = c->fMax_start + alpha*(c->fMax_end - c->fMax_start); const double fMax = c->fMax_start + alpha*(c->fMax_end - c->fMax_start);
...@@ -229,6 +229,8 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti ...@@ -229,6 +229,8 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
} }
else else
{ {
std::cout<<"[InverseDynamicsFormulationAccForce] Remove contact "<<
c->contactLevel->contact.name()<<" at time "<<time<<std::endl;
removeRigidContact(c->contactLevel->contact.name()); removeRigidContact(c->contactLevel->contact.name());
// FIXME: this won't work if multiple contact transitions occur at the same time // FIXME: this won't work if multiple contact transitions occur at the same time
// because after erasing an element the iterator is invalid // because after erasing an element the iterator is invalid
......
...@@ -22,6 +22,7 @@ namespace pininvdyn ...@@ -22,6 +22,7 @@ namespace pininvdyn
namespace tasks namespace tasks
{ {
using namespace pininvdyn::math; using namespace pininvdyn::math;
using namespace pininvdyn::trajectories;
using namespace se3; using namespace se3;
TaskComEquality::TaskComEquality(const std::string & name, TaskComEquality::TaskComEquality(const std::string & name,
...@@ -60,6 +61,21 @@ namespace pininvdyn ...@@ -60,6 +61,21 @@ namespace pininvdyn
m_ref = ref; m_ref = ref;
} }
const TrajectorySample & TaskComEquality::getReference() const
{
return m_ref;
}
const Vector & TaskComEquality::getDesiredAcceleration() const
{
return m_a_des;
}
Vector TaskComEquality::getAcceleration(ConstRefVector dv) const
{
return m_constraint.matrix()*dv - m_drift;
}
const Vector & TaskComEquality::position_error() const const Vector & TaskComEquality::position_error() const
{ {
return m_p_error_vec; return m_p_error_vec;
...@@ -98,17 +114,17 @@ namespace pininvdyn ...@@ -98,17 +114,17 @@ namespace pininvdyn
const ConstraintBase & TaskComEquality::compute(const double t, const ConstraintBase & TaskComEquality::compute(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) const Data & data)
{ {
Vector3 p_com, v_com, drift; Vector3 p_com, v_com;
m_robot.com(data, p_com, v_com, drift); m_robot.com(data, p_com, v_com, m_drift);
// Compute errors // Compute errors
m_p_error = p_com - m_ref.pos; m_p_error = p_com - m_ref.pos;
m_v_error = v_com - m_ref.vel; m_v_error = v_com - m_ref.vel;
Vector3 m_a_des = - m_Kp.cwiseProduct(m_p_error) m_a_des = - m_Kp.cwiseProduct(m_p_error)
- m_Kd.cwiseProduct(m_v_error) - m_Kd.cwiseProduct(m_v_error)
+ m_ref.acc; + m_ref.acc;
m_p_error_vec = m_p_error; m_p_error_vec = m_p_error;
m_v_error_vec = m_v_error; m_v_error_vec = m_v_error;
...@@ -121,7 +137,7 @@ namespace pininvdyn ...@@ -121,7 +137,7 @@ namespace pininvdyn
const Matrix3x & Jcom = m_robot.Jcom(data); const Matrix3x & Jcom = m_robot.Jcom(data);
m_constraint.setMatrix(Jcom); m_constraint.setMatrix(Jcom);
m_constraint.setVector(m_a_des - drift); m_constraint.setVector(m_a_des - m_drift);
return m_constraint; return m_constraint;
} }
......
...@@ -22,6 +22,7 @@ namespace pininvdyn ...@@ -22,6 +22,7 @@ namespace pininvdyn
namespace tasks namespace tasks
{ {
using namespace pininvdyn::math; using namespace pininvdyn::math;
using namespace pininvdyn::trajectories;
using namespace se3; using namespace se3;
TaskJointPosture::TaskJointPosture(const std::string & name, TaskJointPosture::TaskJointPosture(const std::string & name,
...@@ -90,6 +91,21 @@ namespace pininvdyn ...@@ -90,6 +91,21 @@ namespace pininvdyn
m_ref = ref; m_ref = ref;
} }
const TrajectorySample & TaskJointPosture::getReference() const
{
return m_ref;
}
const Vector & TaskJointPosture::getDesiredAcceleration() const
{
return m_a_des;
}
Vector TaskJointPosture::getAcceleration(ConstRefVector dv) const
{
return m_constraint.matrix()*dv;
}
const Vector & TaskJointPosture::position_error() const const Vector & TaskJointPosture::position_error() const
{ {
return m_p_error; return m_p_error;
...@@ -128,16 +144,16 @@ namespace pininvdyn ...@@ -128,16 +144,16 @@ namespace pininvdyn
const ConstraintBase & TaskJointPosture::compute(const double t, const ConstraintBase & TaskJointPosture::compute(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) const Data & data)
{ {
// Compute errors // Compute errors
m_p = q.tail(m_robot.nv()-6); m_p = q.tail(m_robot.nv()-6);
m_v = v.tail(m_robot.nv()-6); m_v = v.tail(m_robot.nv()-6);
m_p_error = m_p - m_ref.pos; m_p_error = m_p - m_ref.pos;
m_v_error = m_v - m_ref.vel; m_v_error = m_v - m_ref.vel;
Vector m_a_des = - m_Kp.cwiseProduct(m_p_error) m_a_des = - m_Kp.cwiseProduct(m_p_error)
- m_Kd.cwiseProduct(m_v_error) - m_Kd.cwiseProduct(m_v_error)
+ m_ref.acc; + m_ref.acc;
for(unsigned int i=0; i<m_activeAxes.size(); i++) for(unsigned int i=0; i<m_activeAxes.size(); i++)
m_constraint.vector()(i) = m_a_des(m_activeAxes(i)); m_constraint.vector()(i) = m_a_des(m_activeAxes(i));
......
...@@ -22,6 +22,7 @@ namespace pininvdyn ...@@ -22,6 +22,7 @@ namespace pininvdyn
namespace tasks namespace tasks
{ {
using namespace pininvdyn::math; using namespace pininvdyn::math;
using namespace pininvdyn::trajectories;
using namespace se3; using namespace se3;
TaskSE3Equality::TaskSE3Equality(const std::string & name, TaskSE3Equality::TaskSE3Equality(const std::string & name,
...@@ -29,7 +30,8 @@ namespace pininvdyn ...@@ -29,7 +30,8 @@ namespace pininvdyn
const std::string & frameName): const std::string & frameName):
TaskMotion(name, robot), TaskMotion(name, robot),
m_frame_name(frameName), m_frame_name(frameName),
m_constraint(name, 6, robot.nv()) m_constraint(name, 6, robot.nv()),
m_ref(12, 6)
{ {
assert(m_robot.model().existFrame(frameName)); assert(m_robot.model().existFrame(frameName));
m_frame_id = m_robot.model().getFrameId(frameName); m_frame_id = m_robot.model().getFrameId(frameName);
...@@ -74,11 +76,17 @@ namespace pininvdyn ...@@ -74,11 +76,17 @@ namespace pininvdyn
void TaskSE3Equality::setReference(TrajectorySample & ref) void TaskSE3Equality::setReference(TrajectorySample & ref)
{ {
m_ref = ref;
vectorToSE3(ref.pos, m_M_ref); vectorToSE3(ref.pos, m_M_ref);
m_v_ref = Motion(ref.vel); m_v_ref = Motion(ref.vel);
m_a_ref = Motion(ref.acc); m_a_ref = Motion(ref.acc);
} }
const TrajectorySample & TaskSE3Equality::getReference() const
{
return m_ref;
}
const Vector & TaskSE3Equality::position_error() const const Vector & TaskSE3Equality::position_error() const
{ {
return m_p_error_vec; return m_p_error_vec;
...@@ -109,6 +117,16 @@ namespace pininvdyn ...@@ -109,6 +117,16 @@ namespace pininvdyn
return m_v_ref_vec; return m_v_ref_vec;
} }
const Vector & TaskSE3Equality::getDesiredAcceleration() const
{
return m_a_des;
}
Vector TaskSE3Equality::getAcceleration(ConstRefVector dv) const
{
return m_constraint.matrix()*dv - m_drift.toVector();
}
const ConstraintBase & TaskSE3Equality::getConstraint() const const ConstraintBase & TaskSE3Equality::getConstraint() const
{ {
return m_constraint; return m_constraint;
...@@ -117,13 +135,13 @@ namespace pininvdyn ...@@ -117,13 +135,13 @@ namespace pininvdyn
const ConstraintBase & TaskSE3Equality::compute(const double t, const ConstraintBase & TaskSE3Equality::compute(const double t,
ConstRefVector q, ConstRefVector q,
ConstRefVector v, ConstRefVector v,
Data & data) const Data & data)
{ {
SE3 oMi; SE3 oMi;
Motion v_frame, drift; Motion v_frame;
m_robot.framePosition(data, m_frame_id, oMi); m_robot.framePosition(data, m_frame_id, oMi);
m_robot.frameVelocity(data, m_frame_id, v_frame); m_robot.frameVelocity(data, m_frame_id, v_frame);
m_robot.frameClassicAcceleration(data, m_frame_id, drift); m_robot.frameClassicAcceleration(data, m_frame_id, m_drift);
// Transformation from local to world // Transformation from local to world
m_wMl.rotation(oMi.rotation()); m_wMl.rotation(oMi.rotation());
...@@ -144,16 +162,16 @@ namespace pininvdyn ...@@ -144,16 +162,16 @@ namespace pininvdyn
#endif #endif
// desired acc in local frame // desired acc in local frame
m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector_impl()) m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
- m_Kd.cwiseProduct(m_v_error.toVector_impl()) - m_Kd.cwiseProduct(m_v_error.toVector())
+ m_wMl.actInv(m_a_ref).toVector_impl(); + m_wMl.actInv(m_a_ref).toVector();
// @todo Since Jacobian computation is cheaper in world frame // @todo Since Jacobian computation is cheaper in world frame
// we could do all computations in world frame // we could do all computations in world frame
m_robot.frameJacobianLocal(data, m_frame_id, m_J); m_robot.frameJacobianLocal(data, m_frame_id, m_J);