Commit abe8dad5 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add unit tests for joint bounds task

parent 4d9808ba
......@@ -26,6 +26,7 @@
#include <tsid/tasks/task-se3-equality.hpp>
#include <tsid/tasks/task-com-equality.hpp>
#include <tsid/tasks/task-joint-posture.hpp>
#include <tsid/tasks/task-joint-bounds.hpp>
#include <tsid/trajectories/trajectory-se3.hpp>
#include <tsid/trajectories/trajectory-euclidian.hpp>
......@@ -272,4 +273,50 @@ BOOST_AUTO_TEST_CASE ( test_task_joint_posture )
}
}
BOOST_AUTO_TEST_CASE ( test_task_joint_bounds )
{
cout<<"\n\n*********** TEST TASK JOINT BOUNDS ***********\n";
vector<string> package_dirs;
package_dirs.push_back(romeo_model_path);
string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
RobotWrapper robot(urdfFileName,
package_dirs,
pinocchio::JointModelFreeFlyer(),
false);
const unsigned int na = robot.nv()-6;
const double dt = 0.001;
cout<<"Gonna create task\n";
TaskJointBounds task("task-joint-bounds", robot, dt);
cout<<"Gonna set limits\n"<<na<<endl;
VectorXd dq_max = VectorXd::Ones(na);
VectorXd dq_min = -dq_max;
task.setVelocityBounds(dq_min, dq_max);
BOOST_CHECK(task.getVelocityLowerBounds().isApprox(dq_min));
BOOST_CHECK(task.getVelocityUpperBounds().isApprox(dq_max));
cout<<"Gonna set up for simulation\n";
double t = 0.0;
VectorXd q = neutral(robot.model());
VectorXd v = VectorXd::Zero(robot.nv());
pinocchio::Data data(robot.model());
for(int i=0; i<max_it; i++)
{
robot.computeAllTerms(data, q, v);
const ConstraintBase & constraint = task.compute(t, q, v, data);
BOOST_CHECK(constraint.rows()==robot.nv());
BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols())==static_cast<tsid::math::Index>(robot.nv()));
BOOST_REQUIRE(isFinite(constraint.lowerBound()));
BOOST_REQUIRE(isFinite(constraint.upperBound()));
BOOST_REQUIRE(isFinite(v));
BOOST_REQUIRE(isFinite(q));
t += dt;
}
}
BOOST_AUTO_TEST_SUITE_END ()
......@@ -26,6 +26,7 @@
#include <tsid/tasks/task-com-equality.hpp>
#include <tsid/tasks/task-se3-equality.hpp>
#include <tsid/tasks/task-joint-posture.hpp>
#include <tsid/tasks/task-joint-bounds.hpp>
#include <tsid/trajectories/trajectory-euclidian.hpp>
#include <tsid/solvers/solver-HQP-factory.hxx>
#include <tsid/solvers/utils.hpp>
......@@ -99,12 +100,13 @@ class StandardRomeoInvDynCtrl
Contact6d * contactLF;
TaskComEquality * comTask;
TaskJointPosture * postureTask;
TaskJointBounds * jointBoundsTask;
Vector q;
Vector v;
pinocchio::SE3 H_rf_ref;
pinocchio::SE3 H_lf_ref;
StandardRomeoInvDynCtrl() : t(0.)
StandardRomeoInvDynCtrl(double dt) : t(0.)
{
vector<string> package_dirs;
package_dirs.push_back(romeo_model_path);
......@@ -162,6 +164,13 @@ class StandardRomeoInvDynCtrl
postureTask->Kp(kp_posture*Vector::Ones(nv-6));
postureTask->Kd(2.0*postureTask->Kp().cwiseSqrt());
tsid->addMotionTask(*postureTask, w_posture, 1);
// Add the joint bounds task
jointBoundsTask = new TaskJointBounds("task-joint-bounds", *robot, dt);
Vector dq_max = 10*Vector::Ones(robot->na());
Vector dq_min = -dq_max;
jointBoundsTask->setVelocityBounds(dq_min, dq_max);
tsid->addMotionTask(*jointBoundsTask, 1.0, 0);
}
};
......@@ -194,7 +203,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
const double w_RF = 1e3;
double t = 0.0;
StandardRomeoInvDynCtrl romeo_inv_dyn;
StandardRomeoInvDynCtrl romeo_inv_dyn(dt);
RobotWrapper & robot = *(romeo_inv_dyn.robot);
InverseDynamicsFormulationAccForce * tsid = romeo_inv_dyn.tsid;
Contact6d & contactRF = *(romeo_inv_dyn.contactRF);
......@@ -323,7 +332,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force )
const unsigned int PRINT_N = 100;
double t = 0.0;
StandardRomeoInvDynCtrl romeo_inv_dyn;
StandardRomeoInvDynCtrl romeo_inv_dyn(dt);
RobotWrapper & robot = *(romeo_inv_dyn.robot);
InverseDynamicsFormulationAccForce * tsid = romeo_inv_dyn.tsid;
Contact6d & contactRF = *(romeo_inv_dyn.contactRF);
......@@ -555,8 +564,8 @@ BOOST_AUTO_TEST_CASE ( test_contact_point_invdyn_formulation_acc_force )
for (int i = 0; i < 4; i++) {
ContactPoint* cp = new ContactPoint("contact_" + contactFrames[i], robot,
contactFrames[i], contactNormal, mu, fMin, fMax);
cp->Kp(kp_contact*Vector::Ones(6));
cp->Kd(2.0*cp->Kp().cwiseSqrt());
cp->Kp(kp_contact*Vector::Ones(3));
cp->Kd(2.0*sqrt(kp_contact)*Vector::Ones(3));
cp->setReference(robot.framePosition(data, robot.model().getFrameId(contactFrames[i])));
cp->useLocalFrame(false);
tsid->addRigidContact(*cp, w_forceReg, 1.0, 1);
......@@ -648,7 +657,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_computation_time )
const double dt = 0.001;
double t = 0.0;
StandardRomeoInvDynCtrl romeo_inv_dyn;
StandardRomeoInvDynCtrl romeo_inv_dyn(dt);
RobotWrapper & robot = *(romeo_inv_dyn.robot);
InverseDynamicsFormulationAccForce * tsid = romeo_inv_dyn.tsid;
TaskComEquality & comTask = *(romeo_inv_dyn.comTask);
......@@ -672,7 +681,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_computation_time )
SolverHQPBase * solver_fast = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
"eiquadprog-fast");
SolverHQPBase * solver_rt =
SolverHQPFactory::createNewSolver<61,18,34>(SOLVER_HQP_EIQUADPROG_RT,
SolverHQPFactory::createNewSolver<61,18,71>(SOLVER_HQP_EIQUADPROG_RT,
"eiquadprog-rt");
solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn());
......
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