resibots/inria_wbc

Generic whole-body controller based on quadratic programming

inria_wbc's Issues

Put the compilation & installation of the example project in the CI

We often forget to test it / compile it and CI is the right tool for this!

Quaternion computation compute_traj is wrong

@paulinejmaurice has noticed a bug in compute_traj from trajectory_handler.hpp
quaternion are computed has an interpolation in minimum_jerk_polynom and are not normalized after
It creates some rotation tracking problem
The following fix is proposed :

inline std::queue<pinocchio::SE3> compute_traj(const pinocchio::SE3& start, const pinocchio::SE3& dest, double dt, double trajectory_duration)
{
    Eigen::Vector3d pos_start, pos_dest, pos_xt;
    pos_start = start.translation();
    pos_dest = dest.translation();
    Eigen::Quaterniond quat_start(start.rotation());
    Eigen::Quaterniond quat_dest(dest.rotation());

    // Interpolate time as min jerk to use in slerp routine (otherwise slerp gives constant angular velocity result)
    Eigen::VectorXd t_start(1), t_dest(1), t_xt(1);
    t_start << 0.;
    t_dest << 1.;

    uint n_steps = std::floor(trajectory_duration / dt);
    std::queue<pinocchio::SE3> trajectory;

    for (uint i = 0; i < n_steps; i++) {
        // Min jerk trajectory for translation
        pos_xt = minimum_jerk_polynom(pos_start, pos_dest, dt * i, trajectory_duration);
        t_xt = minimum_jerk_polynom(t_start, t_dest, dt * i, trajectory_duration);
        // Slerp interpolation for quaternion
        // Eigen::Quaterniond quat_xt = quat_start.slerp((i+1.0)/n_steps, quat_dest);
        Eigen::Quaterniond quat_xt = quat_start.slerp(t_xt(0), quat_dest);
        pinocchio::SE3 xt = pinocchio::SE3(quat_xt, pos_xt);
        trajectory.push(xt);
    }
    return trajectory;

Here the time index is computed according to minimum_jerk_polynom and then used in a slerp interpolation
The computed angle step should then not be constant but should increase at the beginning and decrease at the end of the trajectory

Todo is to check that this is the case

Can not run the example

Hi,

I tried to build the example_project, but I can not run the built example, I tried tutorial_0 and it showed yaml error, should I do any configuration on this or do I made any error

The output shows like this:

utheque: searching for [talos/talos.urdf]
utheque: not an absolute path
utheque: not found in current path [/pkgs/inria_example/build]
utheque: not found in current path/utheque [/pkgs/inria_example/build/utheque]
Utheque: $UTHEQUE_PATH: [/pkgs/install/share]
utheque: not found in $UTHEQUE_PATH
controller dt 0.001
controller urdf /pkgs/install/share/utheque/talos/talos.urdf
terminate called after throwing an instance of 'inria_wbc::Exception'
  what():  inria_wbc:: [yaml-cpp: error at line 0, column 0: bad conversion] when calling: c["check_model_collisions"].as<bool>()	[/pkgs/inria_wbc/src/controllers/controller.cpp:90]

------ stack ------

Aborted (core dumped)

Improve reference tracking

for now, we do not give acceleration references and velocity references in the behaviors

REQUIRED
Add velocity, acceleration references and compare the tracking performance with three cases :

• position ref like it is now
• change the task in TSID (take a new one) that does not have any reference for velocity and acceleration (let the optimizer decide withing the bounds)
• specify acceleration and velocity (which we can compute when we generate the trajectory)

Test test_all_robot fails

------------ SUMMARY ------------
OK  => [franka] cartesian_line.yaml
OK  => [talos] arm.yaml
OK  => [talos] clapping.yaml
OK  => [talos] squat.yaml
OK  => [talos] walk.yamlOK  => [talos] walk_on_spot.yamlOK  => [talos] traj_teleop1.yamlOK  => [talos] active_walk.yamlERROR => [icub] arm.yaml        ERROR Exception:The frame with name 'left_foot' does not exist []  (test_all_robots$
cpp:225)
ERROR => [icub] squat.yaml
        ERROR Exception:The frame with name 'left_foot' does not exist []  (test_all_robots$cpp:225)
ERROR => [icub] walk.yaml
        ERROR Exception:The frame with name 'left_foot' does not exist []  (test_all_robots$
cpp:225)
ERROR => [icub] walk_on_spot.yaml
        ERROR Exception:The frame with name 'left_foot' does not exist []  (test_all_robots$
cpp:225)
ERROR => [icub] traj_teleop1.yaml        ERROR Exception:The frame with name 'left_foot' does not exist []  (test_all_robots$cpp:225)
OK  => [tiago] cartesian_line.yaml

versions - branches

Hi,
Which versions/branches (dart, robot_dart, cxx, etc ...) do you recommend for an Ubuntu 18.04 (manual installation)?
Thanks a lot

Issues with the example_project

• we need to be careful with the base_path (we need to set it in the main)
• for the same reason, the talos.cpp example segv (files are not found apparently) but it works with explicit command line options: ./talos_graphics -c ../etc/test_controller.yaml -b ../etc/test_behavior.yaml

Improve error reporting with boost::stack_trace

https://www.boost.org/doc/libs/1_66_0/doc/html/stacktrace/getting_started.html#stacktrace.getting_started.handle_terminates_aborts_and_seg

(unfortunately, this is unlikely to work with the yaml-cpp errors without some logic on our side).

stabilization and hand contact

check that stabilization force regularization is ok when adding a contact on one hand

check zmp distributor z force

zmp distributor stabilization changes the weights of the contact force regularization task

zmp_w: [1, 1, 1, 2, 2, 2]

And put gives a contact force reference to this task

if tsid and inria_wbc are compiled in release I get warnings

[SolverHQuadProgFast.solver-eiquadprog] Inequality contact_rfoot_force_constraint violated: 629.378
-0.00620184
[SolverHQuadProgFast.solver-eiquadprog] Inequality contact_rfoot_force_constraint violated: 618.634
-0.00248331
[SolverHQuadProgFast.solver-eiquadprog] Inequality contact_rfoot_force_constraint violated: 607.247
-0.00127178
[SolverHQuadProgFast.solver-eiquadprog] Inequality contact_rfoot_force_constraint violated: 595.162
-0.00506521

This needs to be checked

Valgrind errors

Hey

we have a least a few valgrind errors to fix asap.

To reproduce (when compiled in debug):

apt-get install valgrind
valgrind ./talos -b ../etc/talos/walk.yaml -f

Result:

==931== Invalid read of size 16
==931==    at 0x1B4C5E: _mm_loadu_pd (emmintrin.h:131)
==931==    by 0x1B4C5E: double __vector(2) Eigen::internal::ploadu<double __vector(2)>(Eigen::internal::unpacket_traits<double __vector(2)>::type const*) (PacketMath.h:336)
==931==    by 0x215C7A: ploadt<__vector(2) double, 0> (GenericPacketMath.h:465)
==931==    by 0x215C7A: double __vector(2) Eigen::internal::mapbase_evaluator<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >::packet<0, double __vector(2)>(long) const (CoreEvaluators.h:867)
==931==    by 0x21022F: void Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, 1, 0, -1, 1> >, Eigen::internal::evaluator<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >, Eigen::internal::assign_op<double, double>, 0>::assignPacket<16, 0, double __vector(2)>(long) (AssignEvaluator.h:658)
==931==    by 0x20A800: Eigen::internal::dense_assignment_loop<Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, 1, 0, -1, 1> >, Eigen::internal::evaluator<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >, Eigen::internal::assign_op<double, double>, 0>, 3, 0>::run(Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, 1, 0, -1, 1> >, Eigen::internal::evaluator<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >, Eigen::internal::assign_op<double, double>, 0>&) (AssignEvaluator.h:416)
==931==    by 0x20395D: void Eigen::internal::call_dense_assignment_loop<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:741)
==931==    by 0x1FAD56: Eigen::internal::Assignment<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double>, Eigen::internal::Dense2Dense, void>::run(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:879)
==931==    by 0x1F1F87: void Eigen::internal::call_assignment_no_alias<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> const&, Eigen::internal::assign_op<double, double> const&) (AssignEvaluator.h:836)
==931==    by 0x1E6AAB: Eigen::Matrix<double, -1, 1, 0, -1, 1>& Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::_set_noalias<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >(Eigen::DenseBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> > const&) (PlainObjectBase.h:728)
==931==    by 0x1D7021: Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::PlainObjectBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >(Eigen::DenseBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> > const&) (PlainObjectBase.h:537)
==931==    by 0x1C76ED: Eigen::Matrix<double, -1, 1, 0, -1, 1>::Matrix<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> >(Eigen::EigenBase<Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, false> > const&) (Matrix.h:379)
==931==    by 0x1ACAEF: main (talos.cpp:385)
==931==  Address 0xed09550 is 48 bytes inside a block of size 304 free'd
==931==    at 0x4C32D3B: free (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==931==    by 0x1B41A2: Eigen::internal::aligned_free(void*) (Memory.h:177)
==931==    by 0x1BD108: void Eigen::internal::conditional_aligned_free<true>(void*) (Memory.h:230)
==931==    by 0x1CB3F7: void Eigen::internal::conditional_aligned_delete_auto<double, true>(double*, unsigned long) (Memory.h:416)
==931==    by 0x1BD2C2: Eigen::DenseStorage<double, -1, -1, 1, 0>::~DenseStorage() (DenseStorage.h:542)
==931==    by 0x1B543F: Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::~PlainObjectBase() (PlainObjectBase.h:98)
==931==    by 0x1B545B: Eigen::Matrix<double, -1, 1, 0, -1, 1>::~Matrix() (Matrix.h:178)
==931==    by 0x1ACA73: main (talos.cpp:383)
==931==  Block was alloc'd at
==931==    at 0x4C31B0F: malloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==931==    by 0x1B413B: Eigen::internal::aligned_malloc(unsigned long) (Memory.h:159)
==931==    by 0x1BD0EE: void* Eigen::internal::conditional_aligned_malloc<true>(unsigned long) (Memory.h:214)
==931==    by 0x1DB5C6: double* Eigen::internal::conditional_aligned_new_auto<double, true>(unsigned long) (Memory.h:374)
==931==    by 0x1CC01B: Eigen::DenseStorage<double, -1, -1, 1, 0>::resize(long, long, long) (DenseStorage.h:557)
==931==    by 0x1BDE22: Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::resize(long) (PlainObjectBase.h:319)
==931==    by 0x1CC6F7: void Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::_init1<unsigned long>(long, Eigen::internal::enable_if<((((Eigen::DenseBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::{unnamed type#1})-1)!=(1))||(!Eigen::internal::is_convertible<unsigned long, double>::value))&&((!((Eigen::internal::is_same<Eigen::MatrixXpr, Eigen::ArrayXpr>::{unnamed type#1})0))||((({unnamed type#1})-1)==Eigen::Dynamic)), Eigen::internal::is_convertible>::type*) (PlainObjectBase.h:776)
==931==    by 0x1BE8BB: Eigen::Matrix<double, -1, 1, 0, -1, 1>::Matrix<unsigned long>(unsigned long const&) (Matrix.h:296)
==931==    by 0x1B751F: inria_wbc::utils::slice_vec(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, std::vector<int, std::allocator<int> > const&) (utils.hpp:53)
==931==    by 0x1B7738: inria_wbc::controllers::Controller::filter_cmd(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&) const (controller.hpp:114)
==931==    by 0x1ACA45: main (talos.cpp:383)

torque control issue

talos torque control now fails on devel it needs to be fixed

Do not add tasks if weight < 0

This will make it easy to repurpose / configure the controller (better than weight=0).

Improvement: refactor our RobotModel

• use it instead of the TSID robot_wrapper
• merge with the collision checker

update example project

example project is using test_controller.cpp and not talos.cpp

Besides it would be nice to have at least one other simple example

yaml and deterministic tests

@Timothee-ANNE has remarked that test_controller was not deterministic anymore
The issue seems to be that according to the order of the tasks in the yaml files we don't exactly have the same q output
The momentum task also generates non deterministics results, we need to check if those issue are linked

We should also add a test to check determinism

Write a simpler test suite

• instantiate all the robots and all the behaviors
• simply check that they do not fail

(right now, the tests are too strict and we never look at them because they fail too easily).

close the loop (for torque control) for fixed base robots

For now, only TalosPosTracker uses the closed_loop param. This explains why the Franka & Tiago do not work precisely in torque control for now.

Tests are sometimes failing due to COP measurment in stabilization

Check cop filtering + determinism issue #45

sync posture configuration

Automatically take the same init posture configuration from yaml for the posture task and for the config in ros_control

Find better initial position

Find an initial position configuration with an initial Talos COM position at the center of the support polygon

Talos spawning height

With the current height initial value of 1.05847 in etc/talos/configurations.srdf, Talos touches the ground at the 39ms.
By replacing it with 1.051606, Talos touches the floor between 1 and 2 ms.

[minor issue] base_path is ignored when searching for the URDF in controller.cpp

We usually do not see this as we replace the URDF in the robot_dart examples, but we should probably update it.

Stabilizer gains applied in world coordinates

in com_admittance there are have separated gains for x and y axis. The problem is that these are applied to the error in world coordinates. If gains have different values on x and y, then the admittance behavior changes based on the robot orientation in world coordinates.

inria_wbc/src/stabilizers/stabilizer.cpp

Lines 31 to 58 in 9266e64

	void com_admittance(
	double dt,
	const Eigen::VectorXd& p,
	const Eigen::Vector2d& cop_filtered,
	const tsid::trajectories::TrajectorySample& model_current_com,
	const tsid::trajectories::TrajectorySample& com_ref,
	tsid::trajectories::TrajectorySample& se3_sample)
	{
	IWBC_ASSERT("you need 6 coefficient in p for com admittance", p.size() == 6);
	if (std::abs(cop_filtered(0)) >= 10 && std::abs(cop_filtered(1)) >= 10)
	IWBC_ERROR("com_admittance : something is wrong with input cop_filtered, check sensor measurment: ", std::abs(cop_filtered(0)), " ", std::abs(cop_filtered(1)));
	Eigen::Vector2d ref = com_to_zmp(model_current_com); //because this is the target
	Eigen::Vector2d cor = ref.head(2) - cop_filtered;
	Eigen::Vector2d error = p.segment(0, 2).array() * cor.array();
	Eigen::VectorXd ref_m = com_ref.getValue() - Eigen::Vector3d(error(0), error(1), 0);
	error = p.segment(2, 2).array() * cor.array();
	Eigen::VectorXd vref_m = com_ref.getDerivative() - (Eigen::Vector3d(error(0), error(1), 0) / dt);
	error = p.segment(4, 2).array() * cor.array();
	Eigen::VectorXd aref_m = com_ref.getSecondDerivative() - (Eigen::Vector3d(error(0), error(1), 0) / (dt * dt));
	se3_sample.setValue(ref_m);
	se3_sample.setDerivative(vref_m);
	se3_sample.setSecondDerivative(aref_m);

this problem might be in other stabilizer too but I haven't used them and check.

I believe the admittance should be applied in a coordinate frame based on the floating base.

A solution would be to have a yaw rotation that express the forward direction of the robot (in the sample i create it from 2d vector), applied its inverse to the error and then bring it back in world coordinates after the gains product

`

    Eigen::Matrix2d fwd_rotation;
    fwd_rotation.col(0) = forward;
    fwd_rotation.col(1) << -forward(1), forward(0); // rotate forward 90 degrees counterclockwise

    Eigen::Vector2d cor = desired_zmp - center_of_pressure;

    // x-y gains are wrt sagittal and frontal axis of robot (not world coordinate)
    // error is rotated in robot frame, gains are applied then. Finally, the contribution is rotated back in world frame
    Eigen::Vector2d error = fwd_rotation * gains.segment<2>(0).cwiseProduct(fwd_rotation.transpose() * cor);
    Eigen::VectorXd ref_m = com_ref.getValue() - Eigen::Vector3d(error(0), error(1), 0);

    error = fwd_rotation * gains.segment<2>(2).cwiseProduct(fwd_rotation.transpose() * cor);
    Eigen::VectorXd vref_m = com_ref.getDerivative() - (Eigen::Vector3d(error(0), error(1), 0) / dt);

    error = fwd_rotation * gains.segment<2>(4).cwiseProduct(fwd_rotation.transpose() * cor);
    Eigen::VectorXd aref_m = com_ref.getSecondDerivative() - (Eigen::Vector3d(error(0), error(1), 0) / (dt * dt));

    se3_sample.setValue(ref_m);
    se3_sample.setDerivative(vref_m);
    se3_sample.setSecondDerivative(aref_m);`

Add ${CMAKE_DL_LIBS} for stack trace without robot_dart

${CMAKE_DL_LIBS}

Do not add robot_dart as a dependency of the library

Even if robot_dart (and Magnum) are detected, we should not link to it in inria_wbc (only in the examples) to avoid link issues for the users of the library.