Skip to content

GitLab

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

Minor improvement everywhere

parent 7c7038e1
Hide whitespace changes
Inline Side-by-side
include/pininvdyn/contacts/contact-6d.hpp
View file @ 83889f96
......@@ -60,19 +60,19 @@ namespace pininvdyn
virtual const ConstraintBase & computeMotionTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const Data & data);
virtual const ConstraintInequality & computeForceTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const Data & data);
virtual const Matrix & getForceGeneratorMatrix();
virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const Data & data);
const TaskMotion & getMotionTask() const;
const ConstraintBase & getMotionConstraint() const;
......
include/pininvdyn/contacts/contact-base.hpp
View file @ 83889f96
......@@ -61,19 +61,19 @@ namespace pininvdyn
virtual const ConstraintBase & computeMotionTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data) = 0;
const Data & data) = 0;
virtual const ConstraintInequality & computeForceTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data) = 0;
const Data & data) = 0;
virtual const Matrix & getForceGeneratorMatrix() = 0;
virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data) = 0;
ConstRefVector q,
ConstRefVector v,
const Data & data) = 0;
virtual const TaskMotion & getMotionTask() const = 0;
virtual const ConstraintBase & getMotionConstraint() const = 0;
......
include/pininvdyn/tasks/task-base.hpp
View file @ 83889f96
......@@ -53,7 +53,7 @@ namespace pininvdyn
virtual const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data) = 0;
const Data & data) = 0;
virtual const ConstraintBase & getConstraint() const = 0;
......
include/pininvdyn/tasks/task-com-equality.hpp
View file @ 83889f96
......@@ -50,11 +50,15 @@ namespace pininvdyn
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const Data & data);
const ConstraintBase & getConstraint() const;
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 & velocity_error() const;
......@@ -72,6 +76,8 @@ namespace pininvdyn
Vector3 m_Kp;
Vector3 m_Kd;
Vector3 m_p_error, m_v_error;
Vector3 m_a_des;
Vector3 m_drift;
Vector m_p_error_vec, m_v_error_vec;
TrajectorySample m_ref;
ConstraintEquality m_constraint;
......
include/pininvdyn/tasks/task-joint-posture.hpp
View file @ 83889f96
......@@ -47,11 +47,15 @@ namespace pininvdyn
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const Data & data);
const ConstraintBase & getConstraint() const;
void setReference(const TrajectorySample & ref);
const TrajectorySample & getReference() const;
const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const;
const Vector & mask() const;
bool mask(const Vector & mask);
......@@ -73,6 +77,7 @@ namespace pininvdyn
Vector m_Kd;
Vector m_p_error, m_v_error;
Vector m_p, m_v;
Vector m_a_des;
Vector m_mask;
VectorXi m_activeAxes;
TrajectorySample m_ref;
......
include/pininvdyn/tasks/task-motion.hpp
View file @ 83889f96
......@@ -19,6 +19,7 @@
#define __invdyn_task_motion_hpp__
#include <pininvdyn/tasks/task-base.hpp>
#include <pininvdyn/trajectories/trajectory-base.hpp>
namespace pininvdyn
{
......@@ -30,16 +31,24 @@ namespace pininvdyn
public:
typedef pininvdyn::RobotWrapper RobotWrapper;
typedef pininvdyn::math::Vector Vector;
typedef pininvdyn::trajectories::TrajectorySample TrajectorySample;
TaskMotion(const std::string & name,
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 & 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;
};
}
}
......
include/pininvdyn/tasks/task-se3-equality.hpp
View file @ 83889f96
......@@ -50,11 +50,15 @@ namespace pininvdyn
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const Data & data);
const ConstraintBase & getConstraint() const;
void setReference(TrajectorySample & ref);
const TrajectorySample & getReference() const;
const Vector & getDesiredAcceleration() const;
Vector getAcceleration(ConstRefVector dv) const;
const Vector & position_error() const;
const Vector & velocity_error() const;
......@@ -80,8 +84,10 @@ namespace pininvdyn
Vector m_Kp;
Vector m_Kd;
Vector m_a_des;
Motion m_drift;
Matrix6x m_J;
ConstraintEquality m_constraint;
TrajectorySample m_ref;
};
}
......
src/contacts/contact-6d.cpp
View file @ 83889f96
......@@ -198,7 +198,7 @@ void Contact6d::setReference(const SE3 & ref)
const ConstraintBase & Contact6d::computeMotionTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data)
const Data & data)
{
return m_motionTask.compute(t, q, v, data);
}
......@@ -206,7 +206,7 @@ const ConstraintBase & Contact6d::computeMotionTask(const double t,
const ConstraintInequality & Contact6d::computeForceTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data)
const Data & data)
{
return m_forceInequality;
}
......@@ -219,7 +219,7 @@ const Matrix & Contact6d::getForceGeneratorMatrix()
const ConstraintEquality & Contact6d::computeForceRegularizationTask(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data)
const Data & data)
{
return m_forceRegTask;
}
......
src/inverse-dynamics-formulation-acc-force.cpp
View file @ 83889f96
......@@ -221,7 +221,7 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
{
const ContactTransitionInfo * c = *it_ct;
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 fMax = c->fMax_start + alpha*(c->fMax_end - c->fMax_start);
......@@ -229,6 +229,8 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
}
else
{
std::cout<<"[InverseDynamicsFormulationAccForce] Remove contact "<<
c->contactLevel->contact.name()<<" at time "<<time<<std::endl;
removeRigidContact(c->contactLevel->contact.name());
// FIXME: this won't work if multiple contact transitions occur at the same time
// because after erasing an element the iterator is invalid
......
src/tasks/task-com-equality.cpp
View file @ 83889f96
......@@ -22,6 +22,7 @@ namespace pininvdyn
namespace tasks
{
using namespace pininvdyn::math;
using namespace pininvdyn::trajectories;
using namespace se3;
TaskComEquality::TaskComEquality(const std::string & name,
......@@ -60,6 +61,21 @@ namespace pininvdyn
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
{
return m_p_error_vec;
......@@ -98,17 +114,17 @@ namespace pininvdyn
const ConstraintBase & TaskComEquality::compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data)
const Data & data)
{
Vector3 p_com, v_com, drift;
m_robot.com(data, p_com, v_com, drift);
Vector3 p_com, v_com;
m_robot.com(data, p_com, v_com, m_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;
m_a_des = - m_Kp.cwiseProduct(m_p_error)
- m_Kd.cwiseProduct(m_v_error)
+ m_ref.acc;
m_p_error_vec = m_p_error;
m_v_error_vec = m_v_error;
......@@ -121,7 +137,7 @@ namespace pininvdyn
const Matrix3x & Jcom = m_robot.Jcom(data);
m_constraint.setMatrix(Jcom);
m_constraint.setVector(m_a_des - drift);
m_constraint.setVector(m_a_des - m_drift);
return m_constraint;
}
......
src/tasks/task-joint-posture.cpp
View file @ 83889f96
......@@ -22,6 +22,7 @@ namespace pininvdyn
namespace tasks
{
using namespace pininvdyn::math;
using namespace pininvdyn::trajectories;
using namespace se3;
TaskJointPosture::TaskJointPosture(const std::string & name,
......@@ -90,6 +91,21 @@ namespace pininvdyn
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
{
return m_p_error;
......@@ -128,16 +144,16 @@ namespace pininvdyn
const ConstraintBase & TaskJointPosture::compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data)
const Data & data)
{
// Compute errors
m_p = q.tail(m_robot.nv()-6);
m_v = v.tail(m_robot.nv()-6);
m_p_error = m_p - m_ref.pos;
m_v_error = m_v - m_ref.vel;
Vector m_a_des = - m_Kp.cwiseProduct(m_p_error)
- m_Kd.cwiseProduct(m_v_error)
+ m_ref.acc;
m_a_des = - m_Kp.cwiseProduct(m_p_error)
- m_Kd.cwiseProduct(m_v_error)
+ m_ref.acc;
for(unsigned int i=0; i<m_activeAxes.size(); i++)
m_constraint.vector()(i) = m_a_des(m_activeAxes(i));
......
src/tasks/task-se3-equality.cpp
View file @ 83889f96
......@@ -22,6 +22,7 @@ namespace pininvdyn
namespace tasks
{
using namespace pininvdyn::math;
using namespace pininvdyn::trajectories;
using namespace se3;
TaskSE3Equality::TaskSE3Equality(const std::string & name,
......@@ -29,7 +30,8 @@ namespace pininvdyn
const std::string & frameName):
TaskMotion(name, robot),
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));
m_frame_id = m_robot.model().getFrameId(frameName);
......@@ -74,11 +76,17 @@ namespace pininvdyn
void TaskSE3Equality::setReference(TrajectorySample & ref)
{
m_ref = ref;
vectorToSE3(ref.pos, m_M_ref);
m_v_ref = Motion(ref.vel);
m_a_ref = Motion(ref.acc);
}
const TrajectorySample & TaskSE3Equality::getReference() const
{
return m_ref;
}
const Vector & TaskSE3Equality::position_error() const
{
return m_p_error_vec;
......@@ -109,6 +117,16 @@ namespace pininvdyn
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
{
return m_constraint;
......@@ -117,13 +135,13 @@ namespace pininvdyn
const ConstraintBase & TaskSE3Equality::compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data)
const Data & data)
{
SE3 oMi;
Motion v_frame, drift;
Motion v_frame;
m_robot.framePosition(data, m_frame_id, oMi);
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
m_wMl.rotation(oMi.rotation());
......@@ -144,16 +162,16 @@ namespace pininvdyn
#endif
// desired acc in local frame
m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector_impl())
- m_Kd.cwiseProduct(m_v_error.toVector_impl())
+ m_wMl.actInv(m_a_ref).toVector_impl();
m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
- m_Kd.cwiseProduct(m_v_error.toVector())
+ m_wMl.actInv(m_a_ref).toVector();
// @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);
m_constraint.setMatrix(m_J);
m_constraint.setVector(m_a_des - drift.toVector_impl());
m_constraint.setVector(m_a_des - m_drift.toVector());
return m_constraint;
}
......
unittest/invdyn-formulation.cpp
View file @ 83889f96
......@@ -23,6 +23,7 @@
#include <pininvdyn/contacts/contact-6d.hpp>
#include <pininvdyn/inverse-dynamics-formulation-acc-force.hpp>
#include <pininvdyn/tasks/task-com-equality.hpp>
#include <pininvdyn/tasks/task-se3-equality.hpp>
#include <pininvdyn/tasks/task-joint-posture.hpp>
#include <pininvdyn/trajectories/trajectory-euclidian.hpp>
#include <pininvdyn/solvers/solver-HQP-base.hpp>
......@@ -47,6 +48,17 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(is_finite(A), #A<<": "<<A)
#define CHECK_LESS_THAN(A,B) BOOST_CHECK_MESSAGE(A<B, #A<<": "<<A<<">"<<B)
#define REQUIRE_TASK_FINITE(task) REQUIRE_FINITE(task.getConstraint().matrix()); \
REQUIRE_FINITE(task.getConstraint().vector())
#define REQUIRE_CONTACT_FINITE(contact) REQUIRE_FINITE(contact.getMotionConstraint().matrix()); \
REQUIRE_FINITE(contact.getMotionConstraint().vector()); \
REQUIRE_FINITE(contact.getForceConstraint().matrix()); \
REQUIRE_FINITE(contact.getForceConstraint().lowerBound()); \
REQUIRE_FINITE(contact.getForceConstraint().upperBound()); \
REQUIRE_FINITE(contact.getForceRegularizationTask().matrix()); \
REQUIRE_FINITE(contact.getForceRegularizationTask().vector())
class StandardHrp2InvDynCtrl
{
public:
......@@ -145,6 +157,8 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
const unsigned int PRINT_N = 10;
const unsigned int REMOVE_CONTACT_N = 100;
const double CONTACT_TRANSITION_TIME = 1.0;
const double kp_RF = 100.0;
const double w_RF = 1e3;
double t = 0.0;
StandardHrp2InvDynCtrl hrp2_inv_dyn;
......@@ -158,6 +172,20 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
Vector v = hrp2_inv_dyn.v;
const int nv = robot.model().nv;
// Add the right foot task
TaskSE3Equality * rightFootTask = new TaskSE3Equality("task-right-foot",
robot,
hrp2_inv_dyn.rf_frame_name);
rightFootTask->Kp(kp_RF*Vector::Ones(6));
rightFootTask->Kd(2.0*rightFootTask->Kp().cwiseSqrt());
se3::SE3 H_rf_ref = robot.position(invDyn->data(),
robot.model().getJointId(hrp2_inv_dyn.rf_frame_name));
invDyn->addMotionTask(*rightFootTask, w_RF, 1);
TrajectorySample s(12, 6);
se3ToVector(H_rf_ref, s.pos);
rightFootTask->setReference(s);
Vector3 com_ref = robot.com(invDyn->data());
com_ref(1) += 0.1;
TrajectoryBase *trajCom = new TrajectoryEuclidianConstant("traj_com", com_ref);
......@@ -172,13 +200,15 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
"solver-eiquadprog");
solver->resize(invDyn->nVar(), invDyn->nEq(), invDyn->nIn());
Vector tau_old(nv-6);
for(int i=0; i<N_DT; i++)
{
if(i==REMOVE_CONTACT_N)
{
cout<<"Break contact right foot\n";
cout<<"Start breaking contact right foot\n";
invDyn->removeRigidContact(contactRF.name(), CONTACT_TRANSITION_TIME);
}
sampleCom = trajCom->computeNext();
comTask.setReference(sampleCom);
samplePosture = trajPosture->computeNext();
......@@ -188,24 +218,10 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
if(i==0)
cout<< hqpDataToString(hqpData, false)<<endl;
REQUIRE_FINITE(postureTask.getConstraint().matrix());
REQUIRE_FINITE(postureTask.getConstraint().vector());
REQUIRE_FINITE(comTask.getConstraint().matrix());
REQUIRE_FINITE(comTask.getConstraint().vector());
REQUIRE_FINITE(contactRF.getMotionConstraint().matrix());
REQUIRE_FINITE(contactRF.getMotionConstraint().vector());
REQUIRE_FINITE(contactRF.getForceConstraint().matrix());
REQUIRE_FINITE(contactRF.getForceConstraint().lowerBound());
REQUIRE_FINITE(contactRF.getForceConstraint().upperBound());
REQUIRE_FINITE(contactRF.getForceRegularizationTask().matrix());
REQUIRE_FINITE(contactRF.getForceRegularizationTask().vector());
REQUIRE_FINITE(contactLF.getMotionConstraint().matrix());
REQUIRE_FINITE(contactLF.getMotionConstraint().vector());
REQUIRE_FINITE(contactLF.getForceConstraint().matrix());
REQUIRE_FINITE(contactLF.getForceConstraint().lowerBound());
REQUIRE_FINITE(contactLF.getForceConstraint().upperBound());
REQUIRE_FINITE(contactLF.getForceRegularizationTask().matrix());
REQUIRE_FINITE(contactLF.getForceRegularizationTask().vector());
REQUIRE_TASK_FINITE(postureTask);
REQUIRE_TASK_FINITE(comTask);
REQUIRE_CONTACT_FINITE(contactRF);
REQUIRE_CONTACT_FINITE(contactLF);
CHECK_LESS_THAN(contactRF.getMotionTask().position_error().norm(), 1e-3);
CHECK_LESS_THAN(contactLF.getMotionTask().position_error().norm(), 1e-3);
......@@ -217,6 +233,23 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
const Vector & tau = invDyn->getActuatorForces(sol);
const Vector & dv = invDyn->getAccelerations(sol);
if(i>0)
{
CHECK_LESS_THAN((tau-tau_old).norm(), 1e1);
if((tau-tau_old).norm()>1e1) // || (i>=197 && i<=200))
{
// contactRF.computeMotionTask(t, q, v, invDyn->data());
// rightFootTask->compute(t, q, v, invDyn->data());
cout << "Time "<<i<<endl;
cout<<"tau:\n"<<tau.transpose()<<"\ntauOld:\n"<<tau_old.transpose()<<"\n";
// cout << "RF contact task des acc: "<<contactRF.getMotionTask().getDesiredAcceleration().transpose()<<endl;
// cout << "RF contact task acc: "<<contactRF.getMotionTask().getAcceleration(dv).transpose()<<endl;
// cout << "RF motion task des acc: "<<rightFootTask->getDesiredAcceleration().transpose()<<endl;
cout << endl;
}
}
tau_old = tau;
if(i%PRINT_N==0)
{
cout<<"Time "<<i<<endl;
......@@ -306,24 +339,10 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force )
if(i==0)
cout<< hqpDataToString(hqpData, false)<<endl;
REQUIRE_FINITE(postureTask.getConstraint().matrix());
REQUIRE_FINITE(postureTask.getConstraint().vector());
REQUIRE_FINITE(comTask.getConstraint().matrix());
REQUIRE_FINITE(comTask.getConstraint().vector());
REQUIRE_FINITE(contactRF.getMotionConstraint().matrix());
REQUIRE_FINITE(contactRF.getMotionConstraint().vector());
REQUIRE_FINITE(contactRF.getForceConstraint().matrix());
REQUIRE_FINITE(contactRF.getForceConstraint().lowerBound());
REQUIRE_FINITE(contactRF.getForceConstraint().upperBound());
REQUIRE_FINITE(contactRF.getForceRegularizationTask().matrix());
REQUIRE_FINITE(contactRF.getForceRegularizationTask().vector());
REQUIRE_FINITE(contactLF.getMotionConstraint().matrix());
REQUIRE_FINITE(contactLF.getMotionConstraint().vector());
REQUIRE_FINITE(contactLF.getForceConstraint().matrix());
REQUIRE_FINITE(contactLF.getForceConstraint().lowerBound());
REQUIRE_FINITE(contactLF.getForceConstraint().upperBound());
REQUIRE_FINITE(contactLF.getForceRegularizationTask().matrix());
REQUIRE_FINITE(contactLF.getForceRegularizationTask().vector());
REQUIRE_TASK_FINITE(postureTask);
REQUIRE_TASK_FINITE(comTask);
REQUIRE_CONTACT_FINITE(contactRF);
REQUIRE_CONTACT_FINITE(contactLF);
CHECK_LESS_THAN(contactRF.getMotionTask().position_error().norm(), 1e-3);
CHECK_LESS_THAN(contactLF.getMotionTask().position_error().norm(), 1e-3);
......
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