A talos whole body control framework using tsid
Optionally, robot_dart can be used for simulation
You will need 4 main components:
- yaml-cpp
- pinocchio
- tsid
- robot_dart (optional)
Please refer to one of those:
Generic whole-body controller based on quadratic programming
A talos whole body control framework using tsid
Optionally, robot_dart can be used for simulation
You will need 4 main components:
Please refer to one of those:
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
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);`
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
Hi,
Which versions/branches (dart, robot_dart, cxx, etc ...) do you recommend for an Ubuntu 18.04 (manual installation)?
Thanks a lot
Check cop filtering + determinism issue #45
This will make it easy to repurpose / configure the controller (better than weight=0).
Find an initial position configuration with an initial Talos COM position at the center of the support polygon
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.
(right now, the tests are too strict and we never look at them because they fail too easily).
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.
------------ 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
Automatically take the same init posture configuration from yaml for the posture task and for the config in ros_control
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 :
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)
base_path
(we need to set it in the main)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
check that stabilization force regularization is ok when adding a contact on one hand
We often forget to test it / compile it and CI is the right tool for this!
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)
We usually do not see this as we replace the URDF in the robot_dart examples, but we should probably update it.
${CMAKE_DL_LIBS}
For now, only TalosPosTracker uses the closed_loop param. This explains why the Franka & Tiago do not work precisely in torque control for now.
@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
example project is using test_controller.cpp and not talos.cpp
Besides it would be nice to have at least one other simple example
(unfortunately, this is unlikely to work with the yaml-cpp errors without some logic on our side).
talos torque control now fails on devel it needs to be fixed
@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
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.