From 29939e58906bb0f4b45654bfbd89f80ab0f9d0b1 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Sat, 19 Jun 2021 17:10:11 +0200 Subject: [PATCH 01/46] initial addition of the tiago (needs Tiago in robot_dart) --- etc/tiago/cartesian_line.yaml | 5 + etc/tiago/configurations.srdf | 19 +++ etc/tiago/pos_tracker.yaml | 12 ++ etc/tiago/tasks.yaml | 11 ++ src/robot_dart/CMakeLists.txt | 1 + src/robot_dart/tiago.cpp | 282 ++++++++++++++++++++++++++++++++++ 6 files changed, 330 insertions(+) create mode 100644 etc/tiago/cartesian_line.yaml create mode 100644 etc/tiago/configurations.srdf create mode 100644 etc/tiago/pos_tracker.yaml create mode 100644 etc/tiago/tasks.yaml create mode 100644 src/robot_dart/tiago.cpp diff --git a/etc/tiago/cartesian_line.yaml b/etc/tiago/cartesian_line.yaml new file mode 100644 index 00000000..b95236e0 --- /dev/null +++ b/etc/tiago/cartesian_line.yaml @@ -0,0 +1,5 @@ +BEHAVIOR: + name : generic::cartesian + task_name: ee + trajectory_duration: 2 + relative_target: [0.0, 0.5, 1.0] \ No newline at end of file diff --git a/etc/tiago/configurations.srdf b/etc/tiago/configurations.srdf new file mode 100644 index 00000000..ed873c6b --- /dev/null +++ b/etc/tiago/configurations.srdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/etc/tiago/pos_tracker.yaml b/etc/tiago/pos_tracker.yaml new file mode 100644 index 00000000..3748e666 --- /dev/null +++ b/etc/tiago/pos_tracker.yaml @@ -0,0 +1,12 @@ +CONTROLLER: + name: pos-tracker + base_path: /home/pal/inria_wbc/etc/tiago + tasks: tasks.yaml + urdf: tiago_steel.urdf + configurations: configurations.srdf + ref_config: start + floating_base: false + floating_base_joint_name: "" + dt: 0.001 + verbose: false + mimic_dof_names : [] \ No newline at end of file diff --git a/etc/tiago/tasks.yaml b/etc/tiago/tasks.yaml new file mode 100644 index 00000000..0deeb346 --- /dev/null +++ b/etc/tiago/tasks.yaml @@ -0,0 +1,11 @@ +ee: + type: se3 + tracked: arm_4_joint + weight: 100.0 + kp: 30.0 + mask: 111111 +posture: + type: posture + weight: 0.75 + kp: 30.0 + ref: start \ No newline at end of file diff --git a/src/robot_dart/CMakeLists.txt b/src/robot_dart/CMakeLists.txt index 6d21bbe7..11c896f6 100644 --- a/src/robot_dart/CMakeLists.txt +++ b/src/robot_dart/CMakeLists.txt @@ -2,6 +2,7 @@ set(EXAMPLE_LIST talos.cpp franka.cpp + tiago.cpp ) diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp new file mode 100644 index 00000000..1f5b687c --- /dev/null +++ b/src/robot_dart/tiago.cpp @@ -0,0 +1,282 @@ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#ifdef GRAPHIC +#include +#endif + +#include "inria_wbc/behaviors/behavior.hpp" +#include "inria_wbc/controllers/pos_tracker.hpp" +#include "inria_wbc/exceptions.hpp" +#include "inria_wbc/robot_dart/cmd.hpp" + + +int main(int argc, char* argv[]) +{ + try { + // program options + namespace po = boost::program_options; + po::options_description desc("Test_controller options"); + // clang-format off + desc.add_options() + ("actuators,a", po::value()->default_value("servo"), "actuator model torque/velocity/servo (always for position control) [default:servo]") + ("behavior,b", po::value()->default_value("../etc/franka/cartesian_line.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/franka/circular_cartesian.yam]") + ("big_window,b", "use a big window (nicer but slower) [default:true]") + ("check_self_collisions", "check the self collisions (print if a collision)") + ("collision,k", po::value()->default_value("fcl"), "collision engine [default:fcl]") + ("controller,c", po::value()->default_value("../etc/franka/pos_tracker.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/franka/pos_tracker.yaml]") + ("duration,d", po::value()->default_value(20), "duration in seconds [20]") + ("enforce_position,e", po::value()->default_value(true), "enforce the positions of the URDF [default:true]") + ("control_freq", po::value()->default_value(1000), "set the control frequency") + ("sim_freq", po::value()->default_value(1000), "set the simulation frequency") + ("ghost,g", "display the ghost (Pinocchio model)") + ("help,h", "produce help message") + ("mp4,m", po::value(), "save the display to a mp4 video [filename]") + ("push,p", po::value>(), "push the robot at t=x1 0.25 s") + ("verbose,v", "verbose mode (controller)") + ("log,l", po::value>()->default_value(std::vector(),""), + "log the trajectory of a dart body [with urdf names] or timing or CoM or cost, example: -l timing -l com -l lf -l cost_com -l cost_lf") + ; + // clang-format on + po::variables_map vm; + try { + po::store(po::parse_command_line(argc, argv, desc), vm); + po::notify(vm); + } + catch (po::too_many_positional_options_error& e) { + // A positional argument like `opt2=option_value_2` was given + std::cerr << e.what() << std::endl; + std::cerr << desc << std::endl; + return 1; + } + catch (po::error_with_option_name& e) { + // Another usage error occurred + std::cerr << e.what() << std::endl; + std::cerr << desc << std::endl; + return 1; + } + + if (vm.count("help")) { + std::cout << desc << std::endl; + return 0; + } + + // clang-format off + std::cout<< "------ CONFIGURATION ------" << std::endl; + std::ostringstream oss_conf; + for (const auto& kv : vm){ + oss_conf << kv.first << " "; + try { oss_conf << kv.second.as(); + } catch(...) {/* do nothing */ } + try { oss_conf << kv.second.as(); + } catch(...) {/* do nothing */ } + try { oss_conf << kv.second.as(); + } catch(...) {/* do nothing */ } + oss_conf << std::endl; + } + std::cout << oss_conf.str(); + std::cout << "--------------------------" << std::endl; + // clang-format on + + bool verbose = (vm.count("verbose") != 0); + std::map> log_files; + for (auto& x : vm["log"].as>()) + log_files[x] = std::make_shared((x + ".dat").c_str()); + + // dt of the simulation and the controller + int sim_freq = vm["sim_freq"].as(); + float dt = 1.0f/sim_freq; + std::cout << "dt:" << dt << std::endl; + + //////////////////// INIT DART ROBOT ////////////////////////////////////// + std::srand(std::time(NULL)); + + std::vector> packages = {{"tiago_description", "tiago/tiago_description"}}; + std::string urdf = "tiago/tiago_steel.urdf"; + auto robot = std::make_shared(urdf, packages); + robot->fix_to_world(); + robot->set_position_enforced(vm["enforce_position"].as()); + robot->set_actuator_types(vm["actuators"].as()); + + + //////////////////// INIT DART SIMULATION WORLD ////////////////////////////////////// + robot_dart::RobotDARTSimu simu(dt); + simu.set_collision_detector(vm["collision"].as()); + +#ifdef GRAPHIC + robot_dart::gui::magnum::GraphicsConfiguration configuration; + if (vm.count("big_window")) { + configuration.width = 1280; + configuration.height = 960; + } + else { + configuration.width = 800; + configuration.height = 500; + } + + auto graphics = std::make_shared(configuration); + simu.set_graphics(graphics); + graphics->look_at({0., 3., 1.}, {0., 0., 0.}); + if (vm.count("mp4")) + graphics->record_video(vm["mp4"].as()); +#endif + simu.add_robot(robot); + simu.add_checkerboard_floor(); + + ///// CONTROLLER + auto controller_path = vm["controller"].as(); + auto controller_config = IWBC_CHECK(YAML::LoadFile(controller_path)); + + controller_config["CONTROLLER"]["base_path"] = "../etc/tiago";// we assume that we run in ./build + controller_config["CONTROLLER"]["urdf"] = robot->model_filename(); + controller_config["CONTROLLER"]["mimic_dof_names"] = robot->mimic_dof_names(); + controller_config["CONTROLLER"]["verbose"] = verbose; + int control_freq = vm["control_freq"].as(); + + auto controller_name = IWBC_CHECK(controller_config["CONTROLLER"]["name"].as()); + auto controller = inria_wbc::controllers::Factory::instance().create(controller_name, controller_config); + auto controller_pos = std::dynamic_pointer_cast(controller); + IWBC_ASSERT(controller_pos, "we expect a PosTracker here"); + + auto behavior_path = vm["behavior"].as(); + auto behavior_config = IWBC_CHECK(YAML::LoadFile(behavior_path)); + auto behavior_name = IWBC_CHECK(behavior_config["BEHAVIOR"]["name"].as()); + auto behavior = inria_wbc::behaviors::Factory::instance().create(behavior_name, controller, behavior_config); + IWBC_ASSERT(behavior, "invalid behavior"); + + + auto all_dofs = controller->all_dofs(); + auto controllable_dofs = controller->controllable_dofs(); + std::cout<<"Q0:"<q0().transpose()<set_positions(controller->q0(), all_dofs); + + uint ncontrollable = controllable_dofs.size(); + + std::cout<<"# of controllable DOFS:"<() && !simu.graphics()->done()) { + double time_step_solver = 0, time_step_cmd = 0, time_step_simu = 0; + + // update the sensors + inria_wbc::controllers::SensorData sensor_data; + // left foot + sensor_data["lf_torque"] = Eigen::Vector3d::Zero(); + sensor_data["lf_force"] = Eigen::Vector3d::Zero(); + // right foot + sensor_data["rf_torque"] = Eigen::Vector3d::Zero(); + sensor_data["rf_force"] = Eigen::Vector3d::Zero(); + // accelerometer + sensor_data["acceleration"] = Eigen::Vector3d::Zero(); + sensor_data["velocity"] = Eigen::Vector3d::Zero(); + // joint positions (excluding floating base) + sensor_data["positions"] = robot->skeleton()->getPositions().tail(ncontrollable); + + + // step the command + Eigen::VectorXd cmd; + if (simu.schedule(simu.control_freq())) { + auto t1_solver = high_resolution_clock::now(); + behavior->update(sensor_data); + auto q = controller->q(false); + auto t2_solver = high_resolution_clock::now(); + time_step_solver = duration_cast(t2_solver - t1_solver).count(); + + auto t1_cmd = high_resolution_clock::now(); + if (vm["actuators"].as() == "velocity" || vm["actuators"].as() == "servo") + cmd = inria_wbc::robot_dart::compute_velocities(robot->skeleton(), q, 1./control_freq); + else // torque + cmd = inria_wbc::robot_dart::compute_spd(robot->skeleton(), q, 1./control_freq); + auto t2_cmd = high_resolution_clock::now(); + time_step_cmd = duration_cast(t2_cmd - t1_cmd).count(); + + robot->set_commands(controller->filter_cmd(cmd).tail(ncontrollable), controllable_dofs); + ++it_cmd; + + max_time_solver = std::max(time_step_solver, max_time_solver); + min_time_solver = std::min(time_step_solver, min_time_solver); + } + + // step the simulation + { + auto t1_simu = high_resolution_clock::now(); + simu.step_world(); + auto t2_simu = high_resolution_clock::now(); + time_step_simu = duration_cast(t2_simu - t1_simu).count(); + ++it_simu; + } + + // log if needed + for (auto& x : log_files) { + if (x.first == "timing") + (*x.second) << time_step_solver / 1000.0 << "\t" << time_step_cmd / 1000.0 << "\t" << time_step_simu / 1000.0 << std::endl; + else if (x.first == "cmd") + (*x.second) << cmd.transpose() << std::endl; + else + (*x.second) << robot->body_pose(x.first).translation().transpose() << std::endl; + } + // print timing information + time_simu += time_step_simu; + time_cmd += time_step_cmd; + time_solver += time_step_solver; + if (it_simu == 100) { + double t_sim = time_simu / it_simu / 1000.; + double t_cmd = time_cmd / it_cmd / 1000.; + double t_solver = time_solver / it_cmd / 1000.; + + std::cout << "t=" << simu.scheduler().current_time() + << "\tit. simu: " << t_sim << " ms" + << "\tit. solver:" << t_solver << " ms [" << min_time_solver / 1000. << " " << max_time_solver / 1000. << "]" + << "\tit. cmd:" << t_cmd << " ms" + << std::endl; + std::ostringstream oss; + oss.precision(3); + oss << "[Sim: " << t_sim << " ms]" << std::endl; + oss << "[Solver: " << t_solver << " ms]" << std::endl; + oss << "[Cmd: " << t_cmd << " ms]" << std::endl; +#ifdef GRAPHIC + if (!vm.count("mp4")) + simu.set_text_panel(oss.str()); +#endif + it_simu = 0; + it_cmd = 0; + time_cmd = 0; + time_simu = 0; + time_solver = 0; + min_time_solver = 1e10; + max_time_solver = 0; + } + } + } + catch (std::exception& e) { + std::string red = "\x1B[31m"; + std::string rst = "\x1B[0m"; + std::string bold = "\x1B[1m"; + std::cout << red << bold << "Error (exception): " << rst << e.what() << std::endl; + } + return 0; +} From 1e4e12f0f1078fdd651dc8c7a34ab30471cdfb19 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Sat, 19 Jun 2021 18:31:23 +0200 Subject: [PATCH 02/46] minor changes --- etc/tiago/cartesian_line.yaml | 4 ++-- etc/tiago/tasks.yaml | 6 +++--- src/behaviors/generic/cartesian.cpp | 2 ++ 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/etc/tiago/cartesian_line.yaml b/etc/tiago/cartesian_line.yaml index b95236e0..a844b1a1 100644 --- a/etc/tiago/cartesian_line.yaml +++ b/etc/tiago/cartesian_line.yaml @@ -1,5 +1,5 @@ BEHAVIOR: name : generic::cartesian task_name: ee - trajectory_duration: 2 - relative_target: [0.0, 0.5, 1.0] \ No newline at end of file + trajectory_duration: 5 + relative_target: [0.0, 1.0, 0.0] \ No newline at end of file diff --git a/etc/tiago/tasks.yaml b/etc/tiago/tasks.yaml index 0deeb346..1bb077d9 100644 --- a/etc/tiago/tasks.yaml +++ b/etc/tiago/tasks.yaml @@ -1,11 +1,11 @@ ee: type: se3 tracked: arm_4_joint - weight: 100.0 - kp: 30.0 + weight: 1000.0 + kp: 300.0 mask: 111111 posture: type: posture - weight: 0.75 + weight: 0.95 kp: 30.0 ref: start \ No newline at end of file diff --git a/src/behaviors/generic/cartesian.cpp b/src/behaviors/generic/cartesian.cpp index 9660fa07..1dc63d66 100644 --- a/src/behaviors/generic/cartesian.cpp +++ b/src/behaviors/generic/cartesian.cpp @@ -20,6 +20,8 @@ namespace inria_wbc { auto lh_init = std::static_pointer_cast(controller_)->get_se3_ref(task_name_); auto lh_final = lh_init; lh_final.translation() = relative_target_ + lh_init.translation(); + std::cout<<"lh init:"<dt(), trajectory_duration_)); trajectories_.push_back(trajectory_handler::compute_traj(lh_final, lh_init, controller_->dt(), trajectory_duration_)); From f8e955614603e4e676d4549ed8c33cc84623afc4 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Tue, 29 Jun 2021 21:27:03 +0200 Subject: [PATCH 03/46] add some collision with base --- etc/tiago/cartesian_line.yaml | 4 +- etc/tiago/tasks.yaml | 24 +++++++++--- src/behaviors/generic/cartesian.cpp | 1 - src/robot_dart/tiago.cpp | 58 +++++++++++++++++++++++------ 4 files changed, 66 insertions(+), 21 deletions(-) diff --git a/etc/tiago/cartesian_line.yaml b/etc/tiago/cartesian_line.yaml index a844b1a1..4c0fbac2 100644 --- a/etc/tiago/cartesian_line.yaml +++ b/etc/tiago/cartesian_line.yaml @@ -1,5 +1,5 @@ BEHAVIOR: name : generic::cartesian task_name: ee - trajectory_duration: 5 - relative_target: [0.0, 1.0, 0.0] \ No newline at end of file + trajectory_duration: 4 + relative_target: [-0.90, 0.0, -0.50] \ No newline at end of file diff --git a/etc/tiago/tasks.yaml b/etc/tiago/tasks.yaml index 1bb077d9..f6208540 100644 --- a/etc/tiago/tasks.yaml +++ b/etc/tiago/tasks.yaml @@ -1,11 +1,23 @@ ee: type: se3 - tracked: arm_4_joint - weight: 1000.0 - kp: 300.0 + tracked: gripper_link + weight: 100.0 + kp: 1300.0 mask: 111111 posture: type: posture - weight: 0.95 - kp: 30.0 - ref: start \ No newline at end of file + weight: 10.00 + kp: 10.0 + ref: start +bounds: + type: bounds + weight: 10000 +self_collision-gripper: + type: self-collision + tracked: gripper_link + radius: 0.1 + avoided: + base_link: 0.325 + weight: 100 + kp: 700.0 + p: 3 \ No newline at end of file diff --git a/src/behaviors/generic/cartesian.cpp b/src/behaviors/generic/cartesian.cpp index 1dc63d66..1bdf154b 100644 --- a/src/behaviors/generic/cartesian.cpp +++ b/src/behaviors/generic/cartesian.cpp @@ -32,7 +32,6 @@ namespace inria_wbc { { auto ref = current_trajectory_[time_]; std::static_pointer_cast(controller_)->set_se3_ref(ref, task_name_); - controller_->update(sensor_data); time_++; if (time_ == current_trajectory_.size()) { diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index 1f5b687c..07552261 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -20,7 +20,9 @@ #include "inria_wbc/controllers/pos_tracker.hpp" #include "inria_wbc/exceptions.hpp" #include "inria_wbc/robot_dart/cmd.hpp" - +#include "inria_wbc/robot_dart/external_collision_detector.hpp" +#include "inria_wbc/robot_dart/self_collision_detector.hpp" +#include "tsid/tasks/task-self-collision.hpp" int main(int argc, char* argv[]) { @@ -34,13 +36,13 @@ int main(int argc, char* argv[]) ("behavior,b", po::value()->default_value("../etc/franka/cartesian_line.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/franka/circular_cartesian.yam]") ("big_window,b", "use a big window (nicer but slower) [default:true]") ("check_self_collisions", "check the self collisions (print if a collision)") + ("collisions", po::value(), "display the collision shapes for task [name]") ("collision,k", po::value()->default_value("fcl"), "collision engine [default:fcl]") ("controller,c", po::value()->default_value("../etc/franka/pos_tracker.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/franka/pos_tracker.yaml]") ("duration,d", po::value()->default_value(20), "duration in seconds [20]") ("enforce_position,e", po::value()->default_value(true), "enforce the positions of the URDF [default:true]") ("control_freq", po::value()->default_value(1000), "set the control frequency") ("sim_freq", po::value()->default_value(1000), "set the simulation frequency") - ("ghost,g", "display the ghost (Pinocchio model)") ("help,h", "produce help message") ("mp4,m", po::value(), "save the display to a mp4 video [filename]") ("push,p", po::value>(), "push the robot at t=x1 0.25 s") @@ -96,7 +98,7 @@ int main(int argc, char* argv[]) // dt of the simulation and the controller int sim_freq = vm["sim_freq"].as(); - float dt = 1.0f/sim_freq; + float dt = 1.0f / sim_freq; std::cout << "dt:" << dt << std::endl; //////////////////// INIT DART ROBOT ////////////////////////////////////// @@ -109,7 +111,6 @@ int main(int argc, char* argv[]) robot->set_position_enforced(vm["enforce_position"].as()); robot->set_actuator_types(vm["actuators"].as()); - //////////////////// INIT DART SIMULATION WORLD ////////////////////////////////////// robot_dart::RobotDARTSimu simu(dt); simu.set_collision_detector(vm["collision"].as()); @@ -138,7 +139,7 @@ int main(int argc, char* argv[]) auto controller_path = vm["controller"].as(); auto controller_config = IWBC_CHECK(YAML::LoadFile(controller_path)); - controller_config["CONTROLLER"]["base_path"] = "../etc/tiago";// we assume that we run in ./build + controller_config["CONTROLLER"]["base_path"] = "../etc/tiago"; // we assume that we run in ./build controller_config["CONTROLLER"]["urdf"] = robot->model_filename(); controller_config["CONTROLLER"]["mimic_dof_names"] = robot->mimic_dof_names(); controller_config["CONTROLLER"]["verbose"] = verbose; @@ -155,15 +156,14 @@ int main(int argc, char* argv[]) auto behavior = inria_wbc::behaviors::Factory::instance().create(behavior_name, controller, behavior_config); IWBC_ASSERT(behavior, "invalid behavior"); - auto all_dofs = controller->all_dofs(); auto controllable_dofs = controller->controllable_dofs(); - std::cout<<"Q0:"<q0().transpose()<q0().transpose() << std::endl; robot->set_positions(controller->q0(), all_dofs); uint ncontrollable = controllable_dofs.size(); - std::cout<<"# of controllable DOFS:"<> self_collision_spheres; + if (vm.count("collisions")) { + auto task_self_collision = controller_pos->task(vm["collisions"].as()); + for (size_t i = 0; i < task_self_collision->avoided_frames_positions().size(); ++i) { + auto pos = task_self_collision->avoided_frames_positions()[i]; + auto tf = Eigen::Isometry3d(Eigen::Translation3d(pos[0], pos[1], pos[2])); + double r0 = task_self_collision->avoided_frames_r0s()[i]; + auto sphere = robot_dart::Robot::create_ellipsoid(Eigen::Vector3d(r0 * 2, r0 * 2, r0 * 2), tf, "fixed", 1, Eigen::Vector4d(0, 1, 0, 0.5), "self-collision-" + std::to_string(i)); + sphere->set_color_mode("aspect"); + self_collision_spheres.push_back(sphere); + simu.add_visual_robot(self_collision_spheres.back()); + std::cout << "self collision sphere " << i << std::endl; + } + } // the main loop using namespace std::chrono; @@ -196,7 +211,6 @@ int main(int argc, char* argv[]) // joint positions (excluding floating base) sensor_data["positions"] = robot->skeleton()->getPositions().tail(ncontrollable); - // step the command Eigen::VectorXd cmd; if (simu.schedule(simu.control_freq())) { @@ -208,12 +222,11 @@ int main(int argc, char* argv[]) auto t1_cmd = high_resolution_clock::now(); if (vm["actuators"].as() == "velocity" || vm["actuators"].as() == "servo") - cmd = inria_wbc::robot_dart::compute_velocities(robot->skeleton(), q, 1./control_freq); + cmd = inria_wbc::robot_dart::compute_velocities(robot->skeleton(), q, 1. / control_freq); else // torque - cmd = inria_wbc::robot_dart::compute_spd(robot->skeleton(), q, 1./control_freq); + cmd = inria_wbc::robot_dart::compute_spd(robot->skeleton(), q, 1. / control_freq); auto t2_cmd = high_resolution_clock::now(); time_step_cmd = duration_cast(t2_cmd - t1_cmd).count(); - robot->set_commands(controller->filter_cmd(cmd).tail(ncontrollable), controllable_dofs); ++it_cmd; @@ -221,6 +234,27 @@ int main(int argc, char* argv[]) min_time_solver = std::min(time_step_solver, min_time_solver); } + if (simu.schedule(simu.graphics_freq()) && vm.count("collisions")) { + auto controller_pos = std::dynamic_pointer_cast(controller); + auto task_self_collision = controller_pos->task(vm["collisions"].as()); + for (size_t i = 0; i < task_self_collision->avoided_frames_positions().size(); ++i) { + auto cp = self_collision_spheres[i]->base_pose(); + cp.translation() = task_self_collision->avoided_frames_positions()[i]; + //cp.translation()[0] -= 1; // move to the ghost + self_collision_spheres[i]->set_base_pose(cp); + auto bd = self_collision_spheres[i]->skeleton()->getBodyNodes()[0]; + auto visual = bd->getShapeNodesWith()[0]; + visual->getShape()->setDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR); + bool c = task_self_collision->collision(i); + if (c) { + visual->getVisualAspect()->setRGBA(dart::Color::Red(1.0)); + } + else { + visual->getVisualAspect()->setRGBA(dart::Color::Green(1.0)); + } + } + } + // step the simulation { auto t1_simu = high_resolution_clock::now(); From a8adcb3537cece2b07f8b9b17a3d1b34a1c2dd77 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Tue, 29 Jun 2021 21:39:30 +0200 Subject: [PATCH 04/46] better self-collision for tiago --- etc/tiago/frames.yaml | 7 +++++++ etc/tiago/pos_tracker.yaml | 1 + etc/tiago/tasks.yaml | 17 +++++++++++++++++ 3 files changed, 25 insertions(+) create mode 100644 etc/tiago/frames.yaml diff --git a/etc/tiago/frames.yaml b/etc/tiago/frames.yaml new file mode 100644 index 00000000..988790ef --- /dev/null +++ b/etc/tiago/frames.yaml @@ -0,0 +1,7 @@ +# virtual frames +v_torso_1: + ref: "torso_lift_link" + pos: [0.0, 0.0, -0.2] +v_torso_2: + ref: "torso_lift_link" + pos: [0.0, 0.0, -0.4] diff --git a/etc/tiago/pos_tracker.yaml b/etc/tiago/pos_tracker.yaml index 3748e666..da8d1975 100644 --- a/etc/tiago/pos_tracker.yaml +++ b/etc/tiago/pos_tracker.yaml @@ -2,6 +2,7 @@ CONTROLLER: name: pos-tracker base_path: /home/pal/inria_wbc/etc/tiago tasks: tasks.yaml + frames: frames.yaml urdf: tiago_steel.urdf configurations: configurations.srdf ref_config: start diff --git a/etc/tiago/tasks.yaml b/etc/tiago/tasks.yaml index f6208540..6cd1b1ae 100644 --- a/etc/tiago/tasks.yaml +++ b/etc/tiago/tasks.yaml @@ -18,6 +18,23 @@ self_collision-gripper: radius: 0.1 avoided: base_link: 0.325 + torso_lift_link: 0.25 + v_torso_1: 0.2 + v_torso_2: 0.2 + head_2_link: 0.15 + weight: 100 + kp: 700.0 + p: 3 +self_collision-5: + type: self-collision + tracked: arm_5_link + radius: 0.1 + avoided: + base_link: 0.325 + torso_lift_link: 0.25 + v_torso_1: 0.2 + v_torso_2: 0.2 + head_2_link: 0.15 weight: 100 kp: 700.0 p: 3 \ No newline at end of file From 0600391635ea94fbbb920bc00dc5d88149377beb Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Wed, 30 Jun 2021 10:51:40 +0200 Subject: [PATCH 05/46] merge with fix_spd --- etc/tiago/tasks.yaml | 4 ++-- src/robot_dart/tiago.cpp | 13 +++++++++---- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/etc/tiago/tasks.yaml b/etc/tiago/tasks.yaml index 6cd1b1ae..880bea67 100644 --- a/etc/tiago/tasks.yaml +++ b/etc/tiago/tasks.yaml @@ -17,7 +17,7 @@ self_collision-gripper: tracked: gripper_link radius: 0.1 avoided: - base_link: 0.325 + base_link: 0.330 torso_lift_link: 0.25 v_torso_1: 0.2 v_torso_2: 0.2 @@ -30,7 +30,7 @@ self_collision-5: tracked: arm_5_link radius: 0.1 avoided: - base_link: 0.325 + base_link: 0.330 torso_lift_link: 0.25 v_torso_1: 0.2 v_torso_2: 0.2 diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index 07552261..0e5968e4 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -109,8 +109,11 @@ int main(int argc, char* argv[]) auto robot = std::make_shared(urdf, packages); robot->fix_to_world(); robot->set_position_enforced(vm["enforce_position"].as()); - robot->set_actuator_types(vm["actuators"].as()); - + if (vm["actuators"].as() == "spd") + robot->set_actuator_types("torque"); + else + robot->set_actuator_types(vm["actuators"].as()); + //////////////////// INIT DART SIMULATION WORLD ////////////////////////////////////// robot_dart::RobotDARTSimu simu(dt); simu.set_collision_detector(vm["collision"].as()); @@ -222,9 +225,11 @@ int main(int argc, char* argv[]) auto t1_cmd = high_resolution_clock::now(); if (vm["actuators"].as() == "velocity" || vm["actuators"].as() == "servo") - cmd = inria_wbc::robot_dart::compute_velocities(robot->skeleton(), q, 1. / control_freq); + cmd = inria_wbc::robot_dart::compute_velocities(robot, q, 1. / control_freq); + else if (vm["actuators"].as() == "spd") + cmd = inria_wbc::robot_dart::compute_spd(robot, q, 1. / sim_freq); else // torque - cmd = inria_wbc::robot_dart::compute_spd(robot->skeleton(), q, 1. / control_freq); + cmd = controller->tau(false); auto t2_cmd = high_resolution_clock::now(); time_step_cmd = duration_cast(t2_cmd - t1_cmd).count(); robot->set_commands(controller->filter_cmd(cmd).tail(ncontrollable), controllable_dofs); From 4d33678f97a0242745eccbc5e24b050bd18666ab Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Wed, 30 Jun 2021 12:39:36 +0200 Subject: [PATCH 06/46] closed loop --- etc/tiago/pos_tracker.yaml | 1 + src/robot_dart/tiago.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/etc/tiago/pos_tracker.yaml b/etc/tiago/pos_tracker.yaml index da8d1975..ff274872 100644 --- a/etc/tiago/pos_tracker.yaml +++ b/etc/tiago/pos_tracker.yaml @@ -5,6 +5,7 @@ CONTROLLER: frames: frames.yaml urdf: tiago_steel.urdf configurations: configurations.srdf + closed_loop: false ref_config: start floating_base: false floating_base_joint_name: "" diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index 0e5968e4..4fc2c47f 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -36,6 +36,7 @@ int main(int argc, char* argv[]) ("behavior,b", po::value()->default_value("../etc/franka/cartesian_line.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/franka/circular_cartesian.yam]") ("big_window,b", "use a big window (nicer but slower) [default:true]") ("check_self_collisions", "check the self collisions (print if a collision)") + ("closed_loop", "Close the loop with floating base position and joint positions; required for torque control [default: from YAML file]") ("collisions", po::value(), "display the collision shapes for task [name]") ("collision,k", po::value()->default_value("fcl"), "collision engine [default:fcl]") ("controller,c", po::value()->default_value("../etc/franka/pos_tracker.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/franka/pos_tracker.yaml]") @@ -147,6 +148,11 @@ int main(int argc, char* argv[]) controller_config["CONTROLLER"]["mimic_dof_names"] = robot->mimic_dof_names(); controller_config["CONTROLLER"]["verbose"] = verbose; int control_freq = vm["control_freq"].as(); + auto closed_loop = IWBC_CHECK(controller_config["CONTROLLER"]["closed_loop"].as()); + if (vm.count("closed_loop")) { + closed_loop = true; + controller_config["CONTROLLER"]["closed_loop"] = true; + } auto controller_name = IWBC_CHECK(controller_config["CONTROLLER"]["name"].as()); auto controller = inria_wbc::controllers::Factory::instance().create(controller_name, controller_config); From c9e2b66fb37d10762e82dd9bdc2ac1f6d4e1cded Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Sat, 10 Jul 2021 14:43:44 +0200 Subject: [PATCH 07/46] add velocities for closed-loop torque control --- src/robot_dart/tiago.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index 4fc2c47f..076e2e89 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -218,7 +218,8 @@ int main(int argc, char* argv[]) sensor_data["acceleration"] = Eigen::Vector3d::Zero(); sensor_data["velocity"] = Eigen::Vector3d::Zero(); // joint positions (excluding floating base) - sensor_data["positions"] = robot->skeleton()->getPositions().tail(ncontrollable); + sensor_data["positions"] = robot->positions(controller->controllable_dofs(false)); + sensor_data["joint_velocities"] = robot->velocities(controller->controllable_dofs(false)); // step the command Eigen::VectorXd cmd; From bdc40cd9f29d88c6f33bc7dc6d8b993e15c2c1ec Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Tue, 13 Jul 2021 17:23:02 +0200 Subject: [PATCH 08/46] merge with devel + add some error for graphics --- src/robot_dart/franka.cpp | 6 ----- tests/ref_test_franka.yaml | 46 ++++++++++++++++++++++++-------------- tests/test_franka.cpp | 7 +++++- tests/test_talos.cpp | 6 ++++- 4 files changed, 40 insertions(+), 25 deletions(-) diff --git a/src/robot_dart/franka.cpp b/src/robot_dart/franka.cpp index fea22bcf..b6da1628 100644 --- a/src/robot_dart/franka.cpp +++ b/src/robot_dart/franka.cpp @@ -112,12 +112,6 @@ int main(int argc, char* argv[]) robot->set_actuator_types("torque"); else robot->set_actuator_types(vm["actuators"].as()); -<<<<<<< HEAD - - - -======= ->>>>>>> devel //////////////////// INIT DART SIMULATION WORLD ////////////////////////////////////// robot_dart::RobotDARTSimu simu(dt); diff --git a/tests/ref_test_franka.yaml b/tests/ref_test_franka.yaml index 9b10c62a..2ecb07bc 100644 --- a/tests/ref_test_franka.yaml +++ b/tests/ref_test_franka.yaml @@ -1,15 +1,15 @@ -timestamp: 2021-06-29_20-35-51 +timestamp: 2021-07-13_17-08-52 ../../etc/franka/pos_tracker.yaml: ../../etc/franka/cartesian_line.yaml: servo: fcl: franka/franka.urdf: time: - - 2.1072739999999999 - - 0.21072740000000001 - - 0.2008625 - - 0.0098503000000000011 - - 1.4600000000000001e-05 + - 2.2006869999999998 + - 0.22006870000000001 + - 0.20985509999999999 + - 0.0102086 + - 5.0000000000000004e-06 ee: - 0.090085309993550272 - 0.089935963435977151 @@ -17,11 +17,11 @@ timestamp: 2021-06-29_20-35-51 fcl: franka/franka.urdf: time: - - 1.862325 - - 0.18623250000000002 - - 0.17664740000000001 - - 0.0095706000000000003 - - 1.45e-05 + - 2.1372520000000002 + - 0.2137252 + - 0.20162450000000001 + - 0.012031100000000001 + - 6.9599999999999998e-05 ee: - 0.090085309993550272 - 0.08993596343598162 @@ -29,11 +29,23 @@ timestamp: 2021-06-29_20-35-51 fcl: franka/franka.urdf: time: - - 2.2355170000000002 - - 0.22355170000000002 - - 0.18587480000000001 - - 0.0102925 - - 0.0273844 + - 2.4753039999999999 + - 0.24753039999999998 + - 0.20557159999999999 + - 0.0118805 + - 0.030078299999999999 ee: - 0.090085309993550272 - - 0.091376721379293563 \ No newline at end of file + - 0.091376721379293563 + torque: + fcl: + franka/franka.urdf: + time: + - 2.290721 + - 0.2290721 + - 0.2182983 + - 0.0107598 + - 1.4e-05 + ee: + - 0.090085309993550272 + - 0.45739694604192194 \ No newline at end of file diff --git a/tests/test_franka.cpp b/tests/test_franka.cpp index 58c3f0ad..c3510666 100644 --- a/tests/test_franka.cpp +++ b/tests/test_franka.cpp @@ -393,6 +393,7 @@ int main(int argc, char** argv) std::cout << "Single test:" << name << std::endl; auto test1 = utest::make_test(name); #ifdef GRAPHIC + std::cout<<"running without the test suite" << std::endl; test_behavior(test1, args[0], args[1], args[2], args[3], args[4], ref, std::shared_ptr()); #else UTEST_REGISTER(test_suite, test1, test_behavior(test1, args[0], args[1], args[2], args[3], args[4], ref, std::shared_ptr())); @@ -400,6 +401,10 @@ int main(int argc, char** argv) utest::write_report(test_suite, std::cout, true); #endif return 0; + } else { +#ifdef GRAPHIC + IWBC_ERROR("GRAPHICS is possible only in single mode (-s ...)"); +#endif } ///// the default behavior is to run all the combinations in different threads @@ -408,7 +413,7 @@ int main(int argc, char** argv) std::string controller = "../../etc/franka/pos_tracker.yaml"; auto behaviors = {"../../etc/franka/cartesian_line.yaml"}; auto collision = {"fcl"}; - auto actuators = {"servo", "velocity", "spd"}; + auto actuators = {"servo", "velocity", "spd", "torque"}; std::string urdf = "franka/franka.urdf"; (*yout) << y::Key << controller << y::Value << y::BeginMap; diff --git a/tests/test_talos.cpp b/tests/test_talos.cpp index c6cc5632..d354761d 100644 --- a/tests/test_talos.cpp +++ b/tests/test_talos.cpp @@ -518,8 +518,12 @@ int main(int argc, char** argv) utest::write_report(test_suite, std::cout, true); #endif return 0; + } else { +#ifdef GRAPHIC + IWBC_ERROR("GRAPHICS is possible only in single mode (-s ...)"); +#endif } - + ///// the default behavior is to run all the combinations in different threads // this is relative to the "tests" directory From c95db860931edb082e6503575bcb4b0bfa3e782a Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Tue, 13 Jul 2021 17:33:26 +0200 Subject: [PATCH 09/46] fix the franka in torque (tests) --- tests/test_franka.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tests/test_franka.cpp b/tests/test_franka.cpp index c3510666..ff73b6c8 100644 --- a/tests/test_franka.cpp +++ b/tests/test_franka.cpp @@ -166,7 +166,9 @@ void test_behavior(utest::test_t test, sensor_data["acceleration"] = Eigen::Vector3d::Zero(); sensor_data["velocity"] = Eigen::Vector3d::Zero(); // joint positions (excluding floating base) - sensor_data["positions"] = robot->skeleton()->getPositions().tail(ncontrollable); + sensor_data["positions"] = robot->positions(controller->controllable_dofs(false)); + sensor_data["joint_velocities"] = robot->velocities(controller->controllable_dofs(false)); + // command if (simu.schedule(simu.control_freq())) { From 9be2096baf6d6365af95aa31f1e20146ddf85e5d Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Tue, 13 Jul 2021 17:34:57 +0200 Subject: [PATCH 10/46] update the ref --- tests/ref_test_franka.yaml | 46 +++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/tests/ref_test_franka.yaml b/tests/ref_test_franka.yaml index 2ecb07bc..c5025b66 100644 --- a/tests/ref_test_franka.yaml +++ b/tests/ref_test_franka.yaml @@ -1,15 +1,15 @@ -timestamp: 2021-07-13_17-08-52 +timestamp: 2021-07-13_17-33-57 ../../etc/franka/pos_tracker.yaml: ../../etc/franka/cartesian_line.yaml: servo: fcl: franka/franka.urdf: time: - - 2.2006869999999998 - - 0.22006870000000001 - - 0.20985509999999999 - - 0.0102086 - - 5.0000000000000004e-06 + - 2.4332940000000001 + - 0.2433294 + - 0.23061280000000001 + - 0.0126074 + - 0.00010920000000000001 ee: - 0.090085309993550272 - 0.089935963435977151 @@ -17,11 +17,11 @@ timestamp: 2021-07-13_17-08-52 fcl: franka/franka.urdf: time: - - 2.1372520000000002 - - 0.2137252 - - 0.20162450000000001 - - 0.012031100000000001 - - 6.9599999999999998e-05 + - 2.255754 + - 0.22557540000000001 + - 0.21201740000000002 + - 0.0133978 + - 0.00016020000000000002 ee: - 0.090085309993550272 - 0.08993596343598162 @@ -29,11 +29,11 @@ timestamp: 2021-07-13_17-08-52 fcl: franka/franka.urdf: time: - - 2.4753039999999999 - - 0.24753039999999998 - - 0.20557159999999999 - - 0.0118805 - - 0.030078299999999999 + - 2.7272780000000001 + - 0.27272779999999996 + - 0.22469339999999999 + - 0.0146754 + - 0.033359 ee: - 0.090085309993550272 - 0.091376721379293563 @@ -41,11 +41,11 @@ timestamp: 2021-07-13_17-08-52 fcl: franka/franka.urdf: time: - - 2.290721 - - 0.2290721 - - 0.2182983 - - 0.0107598 - - 1.4e-05 + - 2.2669860000000002 + - 0.2266986 + - 0.21316779999999999 + - 0.0135178 + - 1.2999999999999999e-05 ee: - - 0.090085309993550272 - - 0.45739694604192194 \ No newline at end of file + - 0.097305300670452399 + - 0.097160029912438065 \ No newline at end of file From ce3b36cfa181fa7864f61648a0bb26d441213b29 Mon Sep 17 00:00:00 2001 From: Ivan Bergonzani Date: Tue, 24 Aug 2021 10:30:29 +0200 Subject: [PATCH 11/46] fix default yaml file --- src/robot_dart/tiago.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index 076e2e89..1d88651e 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -33,13 +33,13 @@ int main(int argc, char* argv[]) // clang-format off desc.add_options() ("actuators,a", po::value()->default_value("servo"), "actuator model torque/velocity/servo (always for position control) [default:servo]") - ("behavior,b", po::value()->default_value("../etc/franka/cartesian_line.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/franka/circular_cartesian.yam]") + ("behavior,b", po::value()->default_value("../etc/tiago/cartesian_line.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/tiago/circular_cartesian.yam]") ("big_window,b", "use a big window (nicer but slower) [default:true]") ("check_self_collisions", "check the self collisions (print if a collision)") ("closed_loop", "Close the loop with floating base position and joint positions; required for torque control [default: from YAML file]") ("collisions", po::value(), "display the collision shapes for task [name]") ("collision,k", po::value()->default_value("fcl"), "collision engine [default:fcl]") - ("controller,c", po::value()->default_value("../etc/franka/pos_tracker.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/franka/pos_tracker.yaml]") + ("controller,c", po::value()->default_value("../etc/tiago/pos_tracker.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/tiago/pos_tracker.yaml]") ("duration,d", po::value()->default_value(20), "duration in seconds [20]") ("enforce_position,e", po::value()->default_value(true), "enforce the positions of the URDF [default:true]") ("control_freq", po::value()->default_value(1000), "set the control frequency") From de7073b67360f4545264b46d2c2a852bffada71a Mon Sep 17 00:00:00 2001 From: Ivan Bergonzani Date: Tue, 24 Aug 2021 11:01:30 +0200 Subject: [PATCH 12/46] big window fix --- src/robot_dart/tiago.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index 1d88651e..dc6d6384 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -34,7 +34,7 @@ int main(int argc, char* argv[]) desc.add_options() ("actuators,a", po::value()->default_value("servo"), "actuator model torque/velocity/servo (always for position control) [default:servo]") ("behavior,b", po::value()->default_value("../etc/tiago/cartesian_line.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/tiago/circular_cartesian.yam]") - ("big_window,b", "use a big window (nicer but slower) [default:true]") + ("big_window,w", "use a big window (nicer but slower) [default:true]") ("check_self_collisions", "check the self collisions (print if a collision)") ("closed_loop", "Close the loop with floating base position and joint positions; required for torque control [default: from YAML file]") ("collisions", po::value(), "display the collision shapes for task [name]") From b0b5befc29c1f7c89e312214b57e27000498e1fa Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Tue, 25 Jan 2022 11:56:31 +0100 Subject: [PATCH 13/46] better starting configuration --- etc/tiago/configurations.srdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/etc/tiago/configurations.srdf b/etc/tiago/configurations.srdf index ed873c6b..07e090f6 100644 --- a/etc/tiago/configurations.srdf +++ b/etc/tiago/configurations.srdf @@ -11,8 +11,8 @@ - - + + From adfa775bfa10308d12a02089745ccd2a554ed59d Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Sun, 13 Feb 2022 21:44:14 +0100 Subject: [PATCH 14/46] new self collision based on sigmoid --- etc/talos/tasks.yaml | 32 ++++++++-------- include/tsid/tasks/task-self-collision.hpp | 19 ++++++++-- src/controllers/tasks.cpp | 4 +- src/tsid/task-self-collision.cpp | 43 ++++++++++++++++++---- 4 files changed, 69 insertions(+), 29 deletions(-) diff --git a/etc/talos/tasks.yaml b/etc/talos/tasks.yaml index 5fa3d4b4..dcfc8852 100644 --- a/etc/talos/tasks.yaml +++ b/etc/talos/tasks.yaml @@ -75,9 +75,9 @@ contact_rfoot: self_collision-left: type: self-collision tracked: gripper_left_joint - radius: 0.1 + radius: 0.13 avoided: - gripper_right_joint: 0.1 + gripper_right_joint: 0.13 arm_right_5_joint: 0.05 v_leg_right_3: 0.15 v_leg_left_3: 0.15 @@ -89,15 +89,15 @@ self_collision-left: leg_left_1_joint: 0.15 leg_right_4_joint: 0.1 leg_left_4_joint: 0.1 - weight: 500 - kp: 700.0 - p: 3 + weight: 50000 + kp: 50.0 + margin: 0.03 self_collision-right: type: self-collision tracked: gripper_right_joint - radius: 0.1 + radius: 0.13 avoided: - gripper_left_joint: 0.1 + gripper_left_joint: 0.13 arm_left_5_joint: 0.05 v_leg_right_3: 0.15 v_leg_left_3: 0.15 @@ -109,9 +109,9 @@ self_collision-right: leg_left_1_joint: 0.15 leg_right_4_joint: 0.1 leg_left_4_joint: 0.1 - weight: 500 - kp: 700.0 - p: 3 + weight: 50000 + kp: 50.0 + margin: 0.03 self_collision-arm-right: type: self-collision tracked: arm_right_4_joint @@ -120,9 +120,9 @@ self_collision-arm-right: base_link: 0.15 v_base_link_left: 0.115 v_base_link_right: 0.115 - weight: 10 - kp: 10.0 - p: 3 + weight: 10000 + kp: 50.0 + margin: 0.03 self_collision-arm-left: type: self-collision tracked: arm_left_4_joint @@ -131,6 +131,6 @@ self_collision-arm-left: base_link: 0.15 v_base_link_left: 0.115 v_base_link_right: 0.115 - weight: 10 - kp: 10.0 - p: 3 \ No newline at end of file + weight: 10000 + kp: 50.0 + margin: 0.03 \ No newline at end of file diff --git a/include/tsid/tasks/task-self-collision.hpp b/include/tsid/tasks/task-self-collision.hpp index b7cb259d..12bc7795 100644 --- a/include/tsid/tasks/task-self-collision.hpp +++ b/include/tsid/tasks/task-self-collision.hpp @@ -30,6 +30,19 @@ namespace tsid { namespace tasks { + + /// \begin{eqnarray} + /// D_p &=& p - p_0 \textrm{ \emph{(3D-vectors)}}\\ + /// N_p &=& ||D_p|| = ||p - p_0|| = \sqrt{(x-a)^2 + (y-b)^2 + (z-c)^2}\\ + /// E_p &=& \exp(k (N_p-d))\\ + /// c(p) &=& \frac{1}{1 + E_p}\\ + /// \nabla c(p) &=& - \frac{k E_p}{N_p (1 + E_p)^2} D_p\\ + /// \mathcal{H}c(p)&=& \left(\frac{2 k^2 E_p^2}{N^2 (E_p+1)^3} + /// - \frac{k^2 E_p}{N^2 (E_p+1)^2} + /// - \frac{k E_p}{N^{3/2} (E_p+1)^2 }\right) + /// D_p D_p^T + /// - \frac{k E_p}{N_p (E_P+1)^2} I + /// \end{eqnarray} class TaskSelfCollision : public TaskMotion { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -45,7 +58,7 @@ namespace tsid { const std::string& frameName, const std::unordered_map& frames, double radius, - double p); + double margin); virtual ~TaskSelfCollision() {} int dim() const; @@ -86,8 +99,8 @@ namespace tsid { Vector3 m_grad_C; Eigen::Matrix m_Hessian_C; - double m_p; - double m_radius; + double m_margin; // zone of influece + double m_radius; // radius around the body ConstraintEquality m_constraint; Vector3 m_drift; diff --git a/src/controllers/tasks.cpp b/src/controllers/tasks.cpp index 4b2e4eeb..566f450c 100644 --- a/src/controllers/tasks.cpp +++ b/src/controllers/tasks.cpp @@ -305,7 +305,7 @@ namespace inria_wbc { double kp = IWBC_CHECK(node["kp"].as()); auto tracked = IWBC_CHECK(node["tracked"].as()); auto weight = IWBC_CHECK(node["weight"].as()); - auto p = IWBC_CHECK(node["p"].as()); + auto margin = IWBC_CHECK(node["margin"].as()); auto radius = IWBC_CHECK(node["radius"].as()); std::unordered_map avoided; @@ -317,7 +317,7 @@ namespace inria_wbc { // create the task assert(tsid); assert(robot); - auto task = std::make_shared(task_name, *robot, tracked, avoided, radius, p); + auto task = std::make_shared(task_name, *robot, tracked, avoided, radius, margin); task->Kp(kp); task->Kd(2.0 * sqrt(task->Kp())); diff --git a/src/tsid/task-self-collision.cpp b/src/tsid/task-self-collision.cpp index bf8ce7cf..e1004cdd 100644 --- a/src/tsid/task-self-collision.cpp +++ b/src/tsid/task-self-collision.cpp @@ -14,6 +14,7 @@ // tsid If not, see // . // +#undef NDEBUG //#define DEBUG_POS_AVOIDANCE #include @@ -32,12 +33,12 @@ namespace tsid { const std::string& tracked_frame_name, const std::unordered_map& avoided_frames_names, double radius, - double p) + double margin) : TaskMotion(name, robot), m_tracked_frame_name(tracked_frame_name), m_avoided_frames_names(avoided_frames_names), m_radius(radius), - m_p(p), + m_margin(margin), m_constraint(name, 1, robot.nv()), m_Js(avoided_frames_names.size()), m_avoided_frames_positions(avoided_frames_names.size()) @@ -75,6 +76,9 @@ namespace tsid { return m_tracked_frame_id; } + inline double sqr(double x) { return x * x; } + inline double cube(double x) { return x * x * x;} + const ConstraintBase& TaskSelfCollision::compute(const double, ConstRefVector q, ConstRefVector v, @@ -88,8 +92,11 @@ namespace tsid { //a_frame = m_robot.frameAccelerationWorldOriented(data, m_tracked_frame_id); m_robot.frameClassicAcceleration(data, m_tracked_frame_id, a_frame); //a_frame = data.a[m_tracked_frame_id]; - auto pos = oMi.translation(); + assert(!std::isnan(pos[0])); + assert(!std::isnan(pos[1])); + assert(!std::isnan(pos[2])); + m_drift = a_frame.linear(); m_robot.frameJacobianWorld(data, m_tracked_frame_id, m_J); //m_robot.frameJacobianLocal(data, m_tracked_frame_id, m_J); @@ -107,14 +114,17 @@ namespace tsid { // distance with tracked frame const Vector3& pos2 = m_avoided_frames_positions[i]; + assert(!std::isnan(pos2[0])); + assert(!std::isnan(pos2[1])); + assert(!std::isnan(pos2[2])); + Vector3 diff = pos - pos2; double square_norm = diff.dot(diff); double norm = sqrt(square_norm); double r = m_avoided_frames_r0s[i]; double eps = 1e-9; // we consider that we influence if above eps - double p = m_p; - double a = (r + m_radius); // * pow(-log(eps), -1.0 / p); //about 0.5; + double a = (r + m_radius);// * pow(-log(eps), -1.0 / p); //about 0.5; // if in the influence zone if (norm <= a) { @@ -131,9 +141,26 @@ namespace tsid { auto J = J1 - m_Js[i].block(0, 0, 3, m_robot.nv()); Eigen::Vector3d drift = m_drift - a_frame.linear(); - m_C(0, 0) = exp(-pow(norm / a, p)); - m_grad_C = -(p / pow(a, p) * pow(norm, p - 2)) * exp(-pow(norm / a, p)) * diff; - m_Hessian_C = exp(-pow(norm / a, p)) * ((p * p / pow(a, 2 * p) * pow(norm, 2 * p - 4) - p * (p - 2) / pow(a, p) * pow(norm, p - 4)) * diff * diff.transpose() - (p / pow(a, p) * pow(norm, p - 2)) * I); + // m_C(0, 0) = exp(-pow(norm / a, p)); + // m_grad_C = -(p / pow(a, p) * pow(norm, p - 2)) * exp(-pow(norm / a, p)) * diff; + // m_Hessian_C = exp(-pow(norm / a, p)) * ((p * p / pow(a, 2 * p) * pow(norm, 2 * p - 4) - p * (p - 2) / pow(a, p) * pow(norm, p - 4)) * diff * diff.transpose() - (p / pow(a, p) * pow(norm, p - 2)) * I); + + double k = -log(1e-5) / m_margin; + // std::cout<<"k:"< Date: Fri, 18 Feb 2022 15:11:25 +0100 Subject: [PATCH 15/46] first prototype of new collision avoidance repulsors --- etc/talos/frames.yaml | 8 ++++- etc/talos/tasks.yaml | 30 ++++++++-------- src/tsid/task-self-collision.cpp | 59 ++++++++++++++++++++++++-------- 3 files changed, 66 insertions(+), 31 deletions(-) diff --git a/etc/talos/frames.yaml b/etc/talos/frames.yaml index 70c565dd..2477d641 100644 --- a/etc/talos/frames.yaml +++ b/etc/talos/frames.yaml @@ -10,4 +10,10 @@ v_base_link_left: pos: [0.0, -0.1, 0] v_base_link_right: ref: "base_link" - pos: [0, 0.1, 0] \ No newline at end of file + pos: [0, 0.1, 0] +v_right_gripper: + ref: "gripper_right_joint" + pos: [0., 0., -0.09] +v_left_gripper: + ref: "gripper_left_joint" + pos: [0., 0., -0.09] diff --git a/etc/talos/tasks.yaml b/etc/talos/tasks.yaml index dcfc8852..9a4e7360 100644 --- a/etc/talos/tasks.yaml +++ b/etc/talos/tasks.yaml @@ -74,10 +74,10 @@ contact_rfoot: normal: [0, 0, 1] self_collision-left: type: self-collision - tracked: gripper_left_joint - radius: 0.13 + tracked: v_left_gripper + radius: 0.15 avoided: - gripper_right_joint: 0.13 + v_right_gripper: 0.15 arm_right_5_joint: 0.05 v_leg_right_3: 0.15 v_leg_left_3: 0.15 @@ -89,15 +89,15 @@ self_collision-left: leg_left_1_joint: 0.15 leg_right_4_joint: 0.1 leg_left_4_joint: 0.1 - weight: 50000 + weight: 2000 kp: 50.0 - margin: 0.03 + margin: 0.02 self_collision-right: type: self-collision - tracked: gripper_right_joint - radius: 0.13 + tracked: v_right_gripper + radius: 0.15 avoided: - gripper_left_joint: 0.13 + v_left_gripper: 0.15 arm_left_5_joint: 0.05 v_leg_right_3: 0.15 v_leg_left_3: 0.15 @@ -109,9 +109,9 @@ self_collision-right: leg_left_1_joint: 0.15 leg_right_4_joint: 0.1 leg_left_4_joint: 0.1 - weight: 50000 + weight: 2000 kp: 50.0 - margin: 0.03 + margin: 0.02 self_collision-arm-right: type: self-collision tracked: arm_right_4_joint @@ -120,9 +120,9 @@ self_collision-arm-right: base_link: 0.15 v_base_link_left: 0.115 v_base_link_right: 0.115 - weight: 10000 + weight: 1000 kp: 50.0 - margin: 0.03 + margin: 0.02 self_collision-arm-left: type: self-collision tracked: arm_left_4_joint @@ -131,6 +131,6 @@ self_collision-arm-left: base_link: 0.15 v_base_link_left: 0.115 v_base_link_right: 0.115 - weight: 10000 - kp: 50.0 - margin: 0.03 \ No newline at end of file + weight: 1000 + kp: 150.0 + margin: 0.02 \ No newline at end of file diff --git a/src/tsid/task-self-collision.cpp b/src/tsid/task-self-collision.cpp index e1004cdd..d83a4e07 100644 --- a/src/tsid/task-self-collision.cpp +++ b/src/tsid/task-self-collision.cpp @@ -16,7 +16,6 @@ // #undef NDEBUG -//#define DEBUG_POS_AVOIDANCE #include #include "tsid/robots/robot-wrapper.hpp" @@ -141,29 +140,59 @@ namespace tsid { auto J = J1 - m_Js[i].block(0, 0, 3, m_robot.nv()); Eigen::Vector3d drift = m_drift - a_frame.linear(); + // old function // m_C(0, 0) = exp(-pow(norm / a, p)); // m_grad_C = -(p / pow(a, p) * pow(norm, p - 2)) * exp(-pow(norm / a, p)) * diff; // m_Hessian_C = exp(-pow(norm / a, p)) * ((p * p / pow(a, 2 * p) * pow(norm, 2 * p - 4) - p * (p - 2) / pow(a, p) * pow(norm, p - 4)) * diff * diff.transpose() - (p / pow(a, p) * pow(norm, p - 2)) * I); - double k = -log(1e-5) / m_margin; - // std::cout<<"k:"< a) { + // double c_p = exp(-k * (norm - a)); + // m_C(0, 0) = c_p; + // std::cout<<"cp exp:"< Date: Fri, 25 Feb 2022 16:25:04 +0100 Subject: [PATCH 16/46] add a different starting configuration for tiago --- etc/tiago/cartesian_line.yaml | 2 +- .../{configurations.srdf => configurations-elbow-down.srdf} | 4 ++-- etc/tiago/pos_tracker.yaml | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) rename etc/tiago/{configurations.srdf => configurations-elbow-down.srdf} (87%) diff --git a/etc/tiago/cartesian_line.yaml b/etc/tiago/cartesian_line.yaml index 810ca4d4..298ea8b3 100644 --- a/etc/tiago/cartesian_line.yaml +++ b/etc/tiago/cartesian_line.yaml @@ -1,6 +1,6 @@ BEHAVIOR: name : generic::cartesian task_names: [ee] - trajectory_duration: 4 + trajectory_duration: 3 relative_targets: [[-0.90, 0.0, -0.50]] loop: true \ No newline at end of file diff --git a/etc/tiago/configurations.srdf b/etc/tiago/configurations-elbow-down.srdf similarity index 87% rename from etc/tiago/configurations.srdf rename to etc/tiago/configurations-elbow-down.srdf index 07e090f6..31e50cc3 100644 --- a/etc/tiago/configurations.srdf +++ b/etc/tiago/configurations-elbow-down.srdf @@ -8,8 +8,8 @@ - - + + diff --git a/etc/tiago/pos_tracker.yaml b/etc/tiago/pos_tracker.yaml index ff274872..907dc340 100644 --- a/etc/tiago/pos_tracker.yaml +++ b/etc/tiago/pos_tracker.yaml @@ -4,11 +4,11 @@ CONTROLLER: tasks: tasks.yaml frames: frames.yaml urdf: tiago_steel.urdf - configurations: configurations.srdf + configurations: configurations-elbow-down.srdf closed_loop: false ref_config: start floating_base: false floating_base_joint_name: "" dt: 0.001 verbose: false - mimic_dof_names : [] \ No newline at end of file + mimic_dof_names : [] From c8bcf0bc9e71d47685d784e5b1264f455a44f3b5 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Fri, 25 Feb 2022 16:25:18 +0100 Subject: [PATCH 17/46] configuration up --- etc/tiago/configurations-elbow-up.srdf | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 etc/tiago/configurations-elbow-up.srdf diff --git a/etc/tiago/configurations-elbow-up.srdf b/etc/tiago/configurations-elbow-up.srdf new file mode 100644 index 00000000..07e090f6 --- /dev/null +++ b/etc/tiago/configurations-elbow-up.srdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 6ad8ab6089d3087ed6a9641115cc1afd3fee07ea Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Tue, 1 Mar 2022 17:06:53 +0100 Subject: [PATCH 18/46] parse parameters --- etc/talos/frames.yaml | 4 +- etc/talos/tasks.yaml | 48 ++++++++++++++++++---- include/tsid/tasks/task-self-collision.hpp | 4 +- src/controllers/tasks.cpp | 6 ++- src/robot_dart/CMakeLists.txt | 4 +- src/tsid/task-self-collision.cpp | 9 ++-- 6 files changed, 57 insertions(+), 18 deletions(-) diff --git a/etc/talos/frames.yaml b/etc/talos/frames.yaml index 2477d641..d1c46e7c 100644 --- a/etc/talos/frames.yaml +++ b/etc/talos/frames.yaml @@ -12,8 +12,8 @@ v_base_link_right: ref: "base_link" pos: [0, 0.1, 0] v_right_gripper: - ref: "gripper_right_joint" + ref: "gripper_right_base_link" pos: [0., 0., -0.09] v_left_gripper: - ref: "gripper_left_joint" + ref: "gripper_left_base_link" pos: [0., 0., -0.09] diff --git a/etc/talos/tasks.yaml b/etc/talos/tasks.yaml index 9a4e7360..363863cc 100644 --- a/etc/talos/tasks.yaml +++ b/etc/talos/tasks.yaml @@ -75,9 +75,9 @@ contact_rfoot: self_collision-left: type: self-collision tracked: v_left_gripper - radius: 0.15 + radius: 0.12 avoided: - v_right_gripper: 0.15 + v_right_gripper: 0.12 arm_right_5_joint: 0.05 v_leg_right_3: 0.15 v_leg_left_3: 0.15 @@ -91,13 +91,15 @@ self_collision-left: leg_left_4_joint: 0.1 weight: 2000 kp: 50.0 + kd: 250.0 margin: 0.02 + m: 0.2 self_collision-right: type: self-collision tracked: v_right_gripper - radius: 0.15 + radius: 0.12 avoided: - v_left_gripper: 0.15 + v_left_gripper: 0.12 arm_left_5_joint: 0.05 v_leg_right_3: 0.15 v_leg_left_3: 0.15 @@ -111,8 +113,10 @@ self_collision-right: leg_left_4_joint: 0.1 weight: 2000 kp: 50.0 + kd: 250.0 margin: 0.02 -self_collision-arm-right: + m: 0.2 +self_collision-elbow-right: type: self-collision tracked: arm_right_4_joint radius: 0.15 @@ -122,8 +126,10 @@ self_collision-arm-right: v_base_link_right: 0.115 weight: 1000 kp: 50.0 + kd: 250.0 margin: 0.02 -self_collision-arm-left: + m: 0.2 +self_collision-elbow-left: type: self-collision tracked: arm_left_4_joint radius: 0.15 @@ -133,4 +139,32 @@ self_collision-arm-left: v_base_link_right: 0.115 weight: 1000 kp: 150.0 - margin: 0.02 \ No newline at end of file + kd: 250.0 + margin: 0.02 + m: 0.2 +self_collision-wrist-right: + type: self-collision + tracked: arm_right_5_joint + radius: 0.10 + avoided: + base_link: 0.15 + v_base_link_left: 0.115 + v_base_link_right: 0.115 + weight: 1000 + kp: 50.0 + kd: 250.0 + margin: 0.02 + m: 0.2 +self_collision-wrist-left: + type: self-collision + tracked: arm_left_5_joint + radius: 0.10 + avoided: + base_link: 0.15 + v_base_link_left: 0.115 + v_base_link_right: 0.115 + weight: 1000 + kp: 150.0 + kd: 250.0 + margin: 0.02 + m: 0.2 \ No newline at end of file diff --git a/include/tsid/tasks/task-self-collision.hpp b/include/tsid/tasks/task-self-collision.hpp index 12bc7795..25f9291b 100644 --- a/include/tsid/tasks/task-self-collision.hpp +++ b/include/tsid/tasks/task-self-collision.hpp @@ -58,7 +58,8 @@ namespace tsid { const std::string& frameName, const std::unordered_map& frames, double radius, - double margin); + double margin, + double m); virtual ~TaskSelfCollision() {} int dim() const; @@ -101,6 +102,7 @@ namespace tsid { double m_margin; // zone of influece double m_radius; // radius around the body + double m_m = 0.2;// exponent of the asymetric sigmoid, smaller = more asymetric ConstraintEquality m_constraint; Vector3 m_drift; diff --git a/src/controllers/tasks.cpp b/src/controllers/tasks.cpp index 566f450c..5f0f7fa0 100644 --- a/src/controllers/tasks.cpp +++ b/src/controllers/tasks.cpp @@ -303,6 +303,8 @@ namespace inria_wbc { // retrieve parameters from YAML double kp = IWBC_CHECK(node["kp"].as()); + double kd = IWBC_CHECK(node["kd"].as()); + double m = IWBC_CHECK(node["m"].as()); auto tracked = IWBC_CHECK(node["tracked"].as()); auto weight = IWBC_CHECK(node["weight"].as()); auto margin = IWBC_CHECK(node["margin"].as()); @@ -317,9 +319,9 @@ namespace inria_wbc { // create the task assert(tsid); assert(robot); - auto task = std::make_shared(task_name, *robot, tracked, avoided, radius, margin); + auto task = std::make_shared(task_name, *robot, tracked, avoided, radius, margin, m); task->Kp(kp); - task->Kd(2.0 * sqrt(task->Kp())); + task->Kd(kd); // add the task to TSID (side effect, be careful) tsid->addMotionTask(*task, weight, 1); diff --git a/src/robot_dart/CMakeLists.txt b/src/robot_dart/CMakeLists.txt index ffec2a77..b5ad5baf 100644 --- a/src/robot_dart/CMakeLists.txt +++ b/src/robot_dart/CMakeLists.txt @@ -17,14 +17,14 @@ function(compile_examples) set_target_properties(${EXAMPLE_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) target_compile_definitions(${EXAMPLE_NAME} PUBLIC BOOST_MATH_DISABLE_FLOAT128=1) target_compile_features(${EXAMPLE_NAME} PUBLIC cxx_std_14) - target_link_libraries(${EXAMPLE_NAME} PUBLIC inria_wbc Boost::program_options RobotDART::Simu) + target_link_libraries(${EXAMPLE_NAME} PUBLIC inria_wbc Boost::program_options RobotDART::Simu Utheque) if(RobotDART_Magnum_FOUND) message("COMPILING EXAMPLE ${EXAMPLE_NAME}_graphics") add_executable(${EXAMPLE_NAME}_graphics ${EXAMPLE}) set_target_properties(${EXAMPLE_NAME}_graphics PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) target_compile_definitions(${EXAMPLE_NAME}_graphics PUBLIC BOOST_MATH_DISABLE_FLOAT128=1) target_compile_features(${EXAMPLE_NAME}_graphics PUBLIC cxx_std_14) - target_link_libraries(${EXAMPLE_NAME}_graphics PUBLIC inria_wbc RobotDART::Magnum RobotDART::Simu Boost::program_options) + target_link_libraries(${EXAMPLE_NAME}_graphics PUBLIC inria_wbc RobotDART::Magnum RobotDART::Simu Boost::program_options Utheque) endif() endforeach() endfunction() diff --git a/src/tsid/task-self-collision.cpp b/src/tsid/task-self-collision.cpp index d83a4e07..ebdaaef8 100644 --- a/src/tsid/task-self-collision.cpp +++ b/src/tsid/task-self-collision.cpp @@ -32,12 +32,14 @@ namespace tsid { const std::string& tracked_frame_name, const std::unordered_map& avoided_frames_names, double radius, - double margin) + double margin, + double m) : TaskMotion(name, robot), m_tracked_frame_name(tracked_frame_name), m_avoided_frames_names(avoided_frames_names), m_radius(radius), m_margin(margin), + m_m(m), m_constraint(name, 1, robot.nv()), m_Js(avoided_frames_names.size()), m_avoided_frames_positions(avoided_frames_names.size()) @@ -157,13 +159,12 @@ namespace tsid { // m_Hessian_C = 0.1 * h * diff * diff.transpose() - k * e_p / (norm * sqr(e_p + 1.)) * I; /// 5PL - double m = 0.2; - double k = -log(pow(-1e-5 +1., -1. / m) - 1.) / m_margin;//-log(1e-5) / m_margin; + double m = m_m; // e.g. 0.2 + double k = -log(pow(-1e-5 +1., -1. / m) - 1.) / m_margin; double s_p = - 1. / k * log(-1 + pow(2, 1. / m)); double x = k * (norm -a + s_p); double e_p = exp(-x); m_C(0, 0) = 1. - pow(1 + e_p, -m); - //std::cout<<"c(m_margin)):"<<1 - pow(1+exp(-k*(m_margin-s_p)), -m) < Date: Wed, 2 Mar 2022 14:35:22 +0100 Subject: [PATCH 19/46] improve self-collision in tiago --- etc/tiago/cartesian_line.yaml | 4 +-- etc/tiago/frames.yaml | 3 ++ etc/tiago/tasks.yaml | 60 +++++++++++++++++++++++++------- src/robot_dart/tiago.cpp | 4 +-- src/tsid/task-self-collision.cpp | 1 + 5 files changed, 55 insertions(+), 17 deletions(-) diff --git a/etc/tiago/cartesian_line.yaml b/etc/tiago/cartesian_line.yaml index 298ea8b3..a77b989e 100644 --- a/etc/tiago/cartesian_line.yaml +++ b/etc/tiago/cartesian_line.yaml @@ -1,6 +1,6 @@ BEHAVIOR: name : generic::cartesian task_names: [ee] - trajectory_duration: 3 - relative_targets: [[-0.90, 0.0, -0.50]] + trajectory_duration: 1.0 + relative_targets: [[-1.4, 0.0, 1.0]] loop: true \ No newline at end of file diff --git a/etc/tiago/frames.yaml b/etc/tiago/frames.yaml index 988790ef..1e42b7c8 100644 --- a/etc/tiago/frames.yaml +++ b/etc/tiago/frames.yaml @@ -5,3 +5,6 @@ v_torso_1: v_torso_2: ref: "torso_lift_link" pos: [0.0, 0.0, -0.4] +v_head: + ref: "head_2_link" + pos: [0.04, 0.05, 0.0] \ No newline at end of file diff --git a/etc/tiago/tasks.yaml b/etc/tiago/tasks.yaml index 880bea67..3ce8d054 100644 --- a/etc/tiago/tasks.yaml +++ b/etc/tiago/tasks.yaml @@ -1,18 +1,18 @@ ee: type: se3 tracked: gripper_link - weight: 100.0 - kp: 1300.0 + weight: 500.0 + kp: 50.0 mask: 111111 posture: type: posture - weight: 10.00 + weight: 1.00 kp: 10.0 ref: start bounds: type: bounds weight: 10000 -self_collision-gripper: +sc-gripper: type: self-collision tracked: gripper_link radius: 0.1 @@ -21,11 +21,28 @@ self_collision-gripper: torso_lift_link: 0.25 v_torso_1: 0.2 v_torso_2: 0.2 - head_2_link: 0.15 - weight: 100 - kp: 700.0 - p: 3 -self_collision-5: + v_head: 0.15 + weight: 1000 + kp: 50.0 + kd: 500.0 + m: 0.2 + margin: 0.02 +sc-wrist: + type: self-collision + tracked: arm_tool_link + radius: 0.1 + avoided: + base_link: 0.330 + torso_lift_link: 0.25 + v_torso_1: 0.2 + v_torso_2: 0.2 + v_head: 0.15 + weight: 1000 + kp: 50.0 + kd: 500.0 + m: 0.2 + margin: 0.02 +sc-forearm: type: self-collision tracked: arm_5_link radius: 0.1 @@ -34,7 +51,24 @@ self_collision-5: torso_lift_link: 0.25 v_torso_1: 0.2 v_torso_2: 0.2 - head_2_link: 0.15 - weight: 100 - kp: 700.0 - p: 3 \ No newline at end of file + v_head: 0.15 + weight: 1000 + kp: 50.0 + kd: 500.0 + m: 0.2 + margin: 0.02 +sc-elbow: + type: self-collision + tracked: arm_4_link + radius: 0.1 + avoided: + base_link: 0.330 + torso_lift_link: 0.25 + v_torso_1: 0.2 + v_torso_2: 0.2 + v_head: 0.15 + weight: 1000 + kp: 50.0 + kd: 500.0 + m: 0.2 + margin: 0.02 \ No newline at end of file diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index dc6d6384..5f0a8702 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -232,9 +232,9 @@ int main(int argc, char* argv[]) auto t1_cmd = high_resolution_clock::now(); if (vm["actuators"].as() == "velocity" || vm["actuators"].as() == "servo") - cmd = inria_wbc::robot_dart::compute_velocities(robot, q, 1. / control_freq); + cmd = inria_wbc::robot_dart::compute_velocities(robot, q, 1. / control_freq, controllable_dofs); else if (vm["actuators"].as() == "spd") - cmd = inria_wbc::robot_dart::compute_spd(robot, q, 1. / sim_freq); + cmd = inria_wbc::robot_dart::compute_spd(robot, q, 1. / sim_freq, controllable_dofs); else // torque cmd = controller->tau(false); auto t2_cmd = high_resolution_clock::now(); diff --git a/src/tsid/task-self-collision.cpp b/src/tsid/task-self-collision.cpp index ebdaaef8..ff6dd544 100644 --- a/src/tsid/task-self-collision.cpp +++ b/src/tsid/task-self-collision.cpp @@ -55,6 +55,7 @@ namespace tsid { m_Js[i].setZero(6, robot.nv()); for (const auto& it : m_avoided_frames_names) { + assert(it.first != m_tracked_frame_name); assert(m_robot.model().existFrame(it.first)); m_avoided_frames_ids.push_back(m_robot.model().getFrameId(it.first)); m_avoided_frames_r0s.push_back(it.second); From c023b4e2e1ad0b5619c230ce01d378cc42a7cea2 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Sun, 13 Mar 2022 18:06:57 +0100 Subject: [PATCH 20/46] add tracked_frame_position --- include/tsid/tasks/task-self-collision.hpp | 5 ++++- src/tsid/task-self-collision.cpp | 9 ++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/include/tsid/tasks/task-self-collision.hpp b/include/tsid/tasks/task-self-collision.hpp index 25f9291b..4ad693b9 100644 --- a/include/tsid/tasks/task-self-collision.hpp +++ b/include/tsid/tasks/task-self-collision.hpp @@ -78,6 +78,9 @@ namespace tsid { void Kd(const double kd) { m_Kd = kd; } Index frame_id() const; + const pinocchio::SE3& tracked_frame_position() const { + return m_tracked_frame_position; + } const std::vector& avoided_frames_positions() const { return m_avoided_frames_positions; } const std::vector& avoided_frames_r0s() const { return m_avoided_frames_r0s; } bool collision(int i) const { return m_collisions[i]; } @@ -110,7 +113,7 @@ namespace tsid { std::vector m_Js; // jacobian of the other frames std::vector m_avoided_frames_positions; - + pinocchio::SE3 m_tracked_frame_position; std::vector m_collisions; Eigen::MatrixXd m_A; diff --git a/src/tsid/task-self-collision.cpp b/src/tsid/task-self-collision.cpp index ebdaaef8..3f92181d 100644 --- a/src/tsid/task-self-collision.cpp +++ b/src/tsid/task-self-collision.cpp @@ -55,6 +55,7 @@ namespace tsid { m_Js[i].setZero(6, robot.nv()); for (const auto& it : m_avoided_frames_names) { + assert(it.first != m_tracked_frame_name); assert(m_robot.model().existFrame(it.first)); m_avoided_frames_ids.push_back(m_robot.model().getFrameId(it.first)); m_avoided_frames_r0s.push_back(it.second); @@ -86,14 +87,13 @@ namespace tsid { Data& data) { // pos & Jacobian of the tracked frame - SE3 oMi; Motion v_frame; Motion a_frame; - m_robot.framePosition(data, m_tracked_frame_id, oMi); + m_robot.framePosition(data, m_tracked_frame_id, m_tracked_frame_position); //a_frame = m_robot.frameAccelerationWorldOriented(data, m_tracked_frame_id); m_robot.frameClassicAcceleration(data, m_tracked_frame_id, a_frame); //a_frame = data.a[m_tracked_frame_id]; - auto pos = oMi.translation(); + auto pos = m_tracked_frame_position.translation(); assert(!std::isnan(pos[0])); assert(!std::isnan(pos[1])); assert(!std::isnan(pos[2])); @@ -108,6 +108,7 @@ namespace tsid { m_B = Eigen::MatrixXd::Zero(1, 1); std::fill(m_collisions.begin(), m_collisions.end(), false); + SE3 oMi; for (size_t i = 0; i < m_avoided_frames_ids.size(); ++i) { // pos & Jacobian m_robot.framePosition(data, m_avoided_frames_ids[i], oMi); @@ -192,8 +193,6 @@ namespace tsid { // A m_A += m_grad_C.transpose() * J; - m_Kd = 250.;//2*K_p^2 - m_Kp = 50; // B (note: m_drift = dJ(q)*dq) m_B += -((m_Hessian_C * J * v).transpose() * J * v + m_grad_C.transpose() * (-drift + m_Kd * J * v) + m_Kp * m_C); } // else 0 for everything From 2e0470f6f13d7cc093c77784963dbc4dc746a097 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Thu, 24 Mar 2022 14:19:56 +0100 Subject: [PATCH 21/46] switch to urdf without wheel --- etc/tiago/configurations-elbow-down.srdf | 2 +- src/controllers/pos_tracker.cpp | 1 + src/robot_dart/tiago.cpp | 11 +++++------ 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/etc/tiago/configurations-elbow-down.srdf b/etc/tiago/configurations-elbow-down.srdf index 31e50cc3..8dbb1160 100644 --- a/etc/tiago/configurations-elbow-down.srdf +++ b/etc/tiago/configurations-elbow-down.srdf @@ -3,7 +3,7 @@ - + diff --git a/src/controllers/pos_tracker.cpp b/src/controllers/pos_tracker.cpp index 997a9b9d..e2ab1d9c 100644 --- a/src/controllers/pos_tracker.cpp +++ b/src/controllers/pos_tracker.cpp @@ -56,6 +56,7 @@ namespace inria_wbc { auto ref_map = robot_->model().referenceConfigurations; IWBC_ASSERT(ref_map.find(ref_config) != ref_map.end(), "The following reference config is not in ref_map : ", ref_config); q_tsid_ = ref_map[ref_config]; + std::cout<<"q_tsid ref:"< #include -#include +#include #include #include #include @@ -105,9 +105,7 @@ int main(int argc, char* argv[]) //////////////////// INIT DART ROBOT ////////////////////////////////////// std::srand(std::time(NULL)); - std::vector> packages = {{"tiago_description", "tiago/tiago_description"}}; - std::string urdf = "tiago/tiago_steel.urdf"; - auto robot = std::make_shared(urdf, packages); + auto robot = std::make_shared(sim_freq, "tiago/tiago_steel_no_wheel.urdf"); robot->fix_to_world(); robot->set_position_enforced(vm["enforce_position"].as()); if (vm["actuators"].as() == "spd") @@ -167,12 +165,13 @@ int main(int argc, char* argv[]) auto all_dofs = controller->all_dofs(); auto controllable_dofs = controller->controllable_dofs(); - std::cout << "Q0:" << controller->q0().transpose() << std::endl; + std::cout << "Q0: " << controller->q0().transpose() << " [" << controller->q0().size()<<"]"<< std::endl; robot->set_positions(controller->q0(), all_dofs); uint ncontrollable = controllable_dofs.size(); - std::cout << "# of controllable DOFS:" << controllable_dofs.size() << std::endl; + std::cout << "# of controllable DOFS: " << controllable_dofs.size() << std::endl; + std::cout << "# of DOFS: " << all_dofs.size() << std::endl; if (vm.count("log")) { std::ofstream ofs("all_dofs.dat"); From 0c2f55c8e3d4eb5db4f5ca22a521e011366e4c31 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Mon, 4 Apr 2022 20:46:12 +0200 Subject: [PATCH 22/46] rotate the tiago back to be aligned with the model --- src/robot_dart/tiago.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index 9f56a1e1..8352bcd3 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -106,6 +106,7 @@ int main(int argc, char* argv[]) std::srand(std::time(NULL)); auto robot = std::make_shared(sim_freq, "tiago/tiago_steel_no_wheel.urdf"); + robot->skeleton()->setPosition(2, 0.);// so that the model is aligned with the tiago robot->fix_to_world(); robot->set_position_enforced(vm["enforce_position"].as()); if (vm["actuators"].as() == "spd") From b73129782df7721f841b15ea4381acd39430530b Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Fri, 29 Apr 2022 11:24:16 +0200 Subject: [PATCH 23/46] merge --- src/robot_dart/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/robot_dart/CMakeLists.txt b/src/robot_dart/CMakeLists.txt index e001a1fd..8eca99bf 100644 --- a/src/robot_dart/CMakeLists.txt +++ b/src/robot_dart/CMakeLists.txt @@ -4,9 +4,8 @@ set(EXAMPLE_LIST talos_load_external.cpp franka.cpp tiago.cpp - tutorial_0.cpp - icub.cpp tutorial_1.cpp + icub.cpp ) set(NODART_EXAMPLE_LIST From b2e118d163e56f0c203958808c13ded5b41fed5c Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Fri, 29 Apr 2022 15:25:43 +0200 Subject: [PATCH 24/46] collision check for tiago --- etc/tiago/cartesian_line.yaml | 5 +++-- etc/tiago/collisions.yaml | 12 ++++++++++++ etc/tiago/pos_tracker.yaml | 3 +++ etc/tiago/tasks.yaml | 10 ++++++++-- src/robot_dart/tiago.cpp | 26 ++++++++++++++++++++++++++ 5 files changed, 52 insertions(+), 4 deletions(-) create mode 100644 etc/tiago/collisions.yaml diff --git a/etc/tiago/cartesian_line.yaml b/etc/tiago/cartesian_line.yaml index a77b989e..78fc42b2 100644 --- a/etc/tiago/cartesian_line.yaml +++ b/etc/tiago/cartesian_line.yaml @@ -1,6 +1,7 @@ BEHAVIOR: name : generic::cartesian task_names: [ee] - trajectory_duration: 1.0 - relative_targets: [[-1.4, 0.0, 1.0]] + trajectory_duration: 10.0 + relative_targets_pos: [[0., 0.5, 0.9]] + relative_targets_rpy: [[0.0, 0.0, 0.0]] loop: true \ No newline at end of file diff --git a/etc/tiago/collisions.yaml b/etc/tiago/collisions.yaml new file mode 100644 index 00000000..7e56e6a1 --- /dev/null +++ b/etc/tiago/collisions.yaml @@ -0,0 +1,12 @@ +members: + arm: + arm_3_link: [[0., 0. ,0., 0.16], [0., 0. ,-0.1, 0.16]] + arm_4_link: [[0., 0. ,0.025, 0.2], [-0.1, 0.0 ,0.025, 0.2]] + arm_5_link: [[0., 0. , 0.03, 0.18],[0.0, 0.0 ,0.15, 0.18]] + arm_7_link: [[0., 0., 0.1, 0.20], [0., 0., 0.2, 0.10]] + torso: + base_link: [[0., 0., 0.11, 0.67]] + torso_lift_link: [[0., 0., -0.1, 0.5],[0., 0., -0.3, 0.36], [0., 0., -0.45, 0.36], [0., 0., -0.55, 0.36], [0., 0., -0.7, 0.36], [0.2, 0., -0.0, 0.25],] + head_2_link: [[0., 0.05, 0.0, 0.35]] + + \ No newline at end of file diff --git a/etc/tiago/pos_tracker.yaml b/etc/tiago/pos_tracker.yaml index 907dc340..3480d8b0 100644 --- a/etc/tiago/pos_tracker.yaml +++ b/etc/tiago/pos_tracker.yaml @@ -6,9 +6,12 @@ CONTROLLER: urdf: tiago_steel.urdf configurations: configurations-elbow-down.srdf closed_loop: false + solver: eiquadprog ref_config: start floating_base: false floating_base_joint_name: "" dt: 0.001 verbose: false mimic_dof_names : [] + check_model_collisions: true + collision_path: collisions.yaml \ No newline at end of file diff --git a/etc/tiago/tasks.yaml b/etc/tiago/tasks.yaml index 3ce8d054..1ad1796e 100644 --- a/etc/tiago/tasks.yaml +++ b/etc/tiago/tasks.yaml @@ -1,12 +1,18 @@ ee: type: se3 tracked: gripper_link - weight: 500.0 + weight: 1500.0 kp: 50.0 mask: 111111 +head: + type: se3 + tracked: head_2_link + weight: 500.0 + kp: 50.0 + mask: 000111 posture: type: posture - weight: 1.00 + weight: 0.1 kp: 10.0 ref: start bounds: diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index 8352bcd3..cd3e01be 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -22,6 +22,7 @@ #include "inria_wbc/robot_dart/cmd.hpp" #include "inria_wbc/robot_dart/external_collision_detector.hpp" #include "inria_wbc/robot_dart/self_collision_detector.hpp" +#include "inria_wbc/robot_dart/utils.hpp" #include "tsid/tasks/task-self-collision.hpp" int main(int argc, char* argv[]) @@ -37,6 +38,7 @@ int main(int argc, char* argv[]) ("big_window,w", "use a big window (nicer but slower) [default:true]") ("check_self_collisions", "check the self collisions (print if a collision)") ("closed_loop", "Close the loop with floating base position and joint positions; required for torque control [default: from YAML file]") + ("model_collisions",po::value()->default_value(false), "display pinocchio qp model collision spheres") ("collisions", po::value(), "display the collision shapes for task [name]") ("collision,k", po::value()->default_value("fcl"), "collision engine [default:fcl]") ("controller,c", po::value()->default_value("../etc/tiago/pos_tracker.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/tiago/pos_tracker.yaml]") @@ -203,6 +205,13 @@ int main(int argc, char* argv[]) // the main loop using namespace std::chrono; + + // for Pinocchio self-collisions + bool init_model_sphere_collisions = false; + std::vector> spheres; + bool is_colliding = false; + + //////////////////////////////// MAIN LOOP //////////////////////////////// while (simu.scheduler().next_time() < vm["duration"].as() && !simu.graphics()->done()) { double time_step_solver = 0, time_step_cmd = 0, time_step_simu = 0; @@ -244,6 +253,23 @@ int main(int argc, char* argv[]) max_time_solver = std::max(time_step_solver, max_time_solver); min_time_solver = std::min(time_step_solver, min_time_solver); + + + // check the collisions + is_colliding = controller->is_model_colliding(); + if (vm["model_collisions"].as()) { + auto spherical_members = controller->collision_check().spherical_members(); + auto sphere_color = dart::Color::Green(0.5); + + if (init_model_sphere_collisions == false) { + spheres = inria_wbc::robot_dart::create_spherical_members(spherical_members, simu, sphere_color); + init_model_sphere_collisions = true; + } + else { + inria_wbc::robot_dart::update_spherical_members(spherical_members, spheres, sphere_color, is_colliding, controller->collision_check().collision_index(), Eigen::VectorXd::Zero(3)); + } + } + } if (simu.schedule(simu.graphics_freq()) && vm.count("collisions")) { From d0749b5d17321fb273c70473f693fe99ecfa6e43 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Fri, 29 Apr 2022 16:20:00 +0200 Subject: [PATCH 25/46] improve spheres for collision check --- etc/tiago/cartesian_line.yaml | 2 +- etc/tiago/collisions.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/etc/tiago/cartesian_line.yaml b/etc/tiago/cartesian_line.yaml index 78fc42b2..b7129ae3 100644 --- a/etc/tiago/cartesian_line.yaml +++ b/etc/tiago/cartesian_line.yaml @@ -1,7 +1,7 @@ BEHAVIOR: name : generic::cartesian task_names: [ee] - trajectory_duration: 10.0 + trajectory_duration: 100.0 relative_targets_pos: [[0., 0.5, 0.9]] relative_targets_rpy: [[0.0, 0.0, 0.0]] loop: true \ No newline at end of file diff --git a/etc/tiago/collisions.yaml b/etc/tiago/collisions.yaml index 7e56e6a1..1dbfb772 100644 --- a/etc/tiago/collisions.yaml +++ b/etc/tiago/collisions.yaml @@ -1,12 +1,12 @@ members: arm: - arm_3_link: [[0., 0. ,0., 0.16], [0., 0. ,-0.1, 0.16]] + arm_3_link: [[0., 0. ,0., 0.13], [0., 0. ,-0.1, 0.16]] arm_4_link: [[0., 0. ,0.025, 0.2], [-0.1, 0.0 ,0.025, 0.2]] arm_5_link: [[0., 0. , 0.03, 0.18],[0.0, 0.0 ,0.15, 0.18]] arm_7_link: [[0., 0., 0.1, 0.20], [0., 0., 0.2, 0.10]] torso: base_link: [[0., 0., 0.11, 0.67]] - torso_lift_link: [[0., 0., -0.1, 0.5],[0., 0., -0.3, 0.36], [0., 0., -0.45, 0.36], [0., 0., -0.55, 0.36], [0., 0., -0.7, 0.36], [0.2, 0., -0.0, 0.25],] + torso_lift_link: [[0.0, 0., 0.03, 0.52],[0., 0., -0.2, 0.36],[0., 0., -0.3, 0.36], [0., 0., -0.45, 0.36], [0., 0., -0.55, 0.36], [0., 0., -0.7, 0.36]] head_2_link: [[0., 0.05, 0.0, 0.35]] \ No newline at end of file From d526013e7bd7b435d750d983c95a36521f9da9e3 Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Tue, 10 May 2022 17:59:27 +0200 Subject: [PATCH 26/46] small fix --- include/inria_wbc/estimators/filtering.hpp | 3 ++- src/controllers/humanoid_pos_tracker.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/include/inria_wbc/estimators/filtering.hpp b/include/inria_wbc/estimators/filtering.hpp index fcd575dc..dc6f6c3b 100644 --- a/include/inria_wbc/estimators/filtering.hpp +++ b/include/inria_wbc/estimators/filtering.hpp @@ -34,7 +34,8 @@ class Filter int get_num_var() const { return _nvar; } int get_window_size() const { return _wsize; } - bool data_ready() const { return (_cnt >= _wsize);} + bool data_ready() const { return (_cnt >= _wsize); } + Eigen::VectorXd filtered() const { return _filtered; } Eigen::VectorXd filter(const Eigen::VectorXd& sample) { diff --git a/src/controllers/humanoid_pos_tracker.cpp b/src/controllers/humanoid_pos_tracker.cpp index a47399f1..c868298f 100644 --- a/src/controllers/humanoid_pos_tracker.cpp +++ b/src/controllers/humanoid_pos_tracker.cpp @@ -156,9 +156,10 @@ namespace inria_wbc { auto torso_ref = get_full_se3_ref("torso"); if (_use_stabilizer) { + IWBC_ASSERT(sensor_data.find("lf_force") != sensor_data.end(), "the stabilizer needs the LF force"); + IWBC_ASSERT(sensor_data.find("rf_force") != sensor_data.end(), "the stabilizer needs the RF force"); IWBC_ASSERT(sensor_data.find("lf_torque") != sensor_data.end(), "the stabilizer needs the LF torque"); IWBC_ASSERT(sensor_data.find("rf_torque") != sensor_data.end(), "the stabilizer needs the RF torque"); - IWBC_ASSERT(sensor_data.find("velocity") != sensor_data.end(), "the stabilizer needs the velocity"); IWBC_ASSERT(sensor_data.find("imu_vel") != sensor_data.end(), "the stabilizer imu needs angular velocity"); // we retrieve the tracked frame from the contact task as the frames have different names in different robots From 89dc4d37302843028a3e5fa273cdd8f1e3bf5eab Mon Sep 17 00:00:00 2001 From: Olivier Rochel Date: Fri, 13 May 2022 10:22:57 +0200 Subject: [PATCH 27/46] controllers: add missing header in humanoid_pos_tracker.hpp --- include/inria_wbc/controllers/humanoid_pos_tracker.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/inria_wbc/controllers/humanoid_pos_tracker.hpp b/include/inria_wbc/controllers/humanoid_pos_tracker.hpp index 901c6699..d939bf36 100644 --- a/include/inria_wbc/controllers/humanoid_pos_tracker.hpp +++ b/include/inria_wbc/controllers/humanoid_pos_tracker.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace inria_wbc { namespace controllers { From 9cc7b7285f8dd501f0c7949feef80f39d1d61a44 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Thu, 26 May 2022 23:06:55 +0200 Subject: [PATCH 28/46] new test (simpler, more reliable) --- etc/franka/pos_tracker.yaml | 1 + etc/icub/traj_teleop1.yaml | 2 +- include/inria_wbc/utils/timer.hpp | 18 +- tests/CMakeLists.txt | 2 +- tests/test_all_robots.cpp | 303 ++++++++++++++++++++++++++++++ 5 files changed, 317 insertions(+), 9 deletions(-) create mode 100644 tests/test_all_robots.cpp diff --git a/etc/franka/pos_tracker.yaml b/etc/franka/pos_tracker.yaml index 659da790..3645076d 100644 --- a/etc/franka/pos_tracker.yaml +++ b/etc/franka/pos_tracker.yaml @@ -1,6 +1,7 @@ CONTROLLER: name: pos-tracker base_path: /home/pal/inria_wbc/etc/franka + solver: eiquadprog tasks: tasks.yaml urdf: franka.urdf configurations: configurations.srdf diff --git a/etc/icub/traj_teleop1.yaml b/etc/icub/traj_teleop1.yaml index f755e8df..1f224462 100644 --- a/etc/icub/traj_teleop1.yaml +++ b/etc/icub/traj_teleop1.yaml @@ -1,5 +1,5 @@ BEHAVIOR: name : generic::cartesian_traj - trajectories: traj_dance/traj_dance.yaml + trajectories: traj_teleop1/trajectory.yaml loop: true scale: 0.75 diff --git a/include/inria_wbc/utils/timer.hpp b/include/inria_wbc/utils/timer.hpp index 80e0e077..daeabb5f 100644 --- a/include/inria_wbc/utils/timer.hpp +++ b/include/inria_wbc/utils/timer.hpp @@ -7,6 +7,12 @@ namespace inria_wbc { namespace utils { class Timer { public: + struct info_t { + int iterations; + double time; + double min_time; + double max_time; + }; void begin(const std::string& name) { _start[name] = std::chrono::high_resolution_clock::now(); @@ -50,16 +56,14 @@ namespace inria_wbc { _data.clear(); } int iteration() const { return _k; } - + const info_t& operator[](const std::string& name) const { + assert(_data.find(name) != _data.end()); + return _data.at(name); + } protected: int _k = 1; using time_t = std::chrono::high_resolution_clock::time_point; - struct info_t { - int iterations; - double time; - double min_time; - double max_time; - }; + std::unordered_map _start; std::unordered_map _data; }; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 638d645c..15c3693d 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -8,7 +8,7 @@ set(BINARY ${CMAKE_PROJECT_NAME}_test) # the tests require Dart to run if(${COMPILE_ROBOT_DART_EXAMPLE} STREQUAL "ON") find_package(RobotDART REQUIRED COMPONENTS Simu OPTIONAL_COMPONENTS Magnum) - set(TEST_SOURCES test_determinism.cpp test_talos.cpp test_franka.cpp test_robot_model.cpp) + set(TEST_SOURCES test_determinism.cpp test_talos.cpp test_franka.cpp test_robot_model.cpp test_all_robots.cpp) endif() foreach(TEST ${TEST_SOURCES}) diff --git a/tests/test_all_robots.cpp b/tests/test_all_robots.cpp new file mode 100644 index 00000000..bc0bf274 --- /dev/null +++ b/tests/test_all_robots.cpp @@ -0,0 +1,303 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "inria_wbc/behaviors/behavior.hpp" +#include "inria_wbc/robot_dart/cmd.hpp" + +#include "inria_wbc/behaviors/behavior.hpp" +#include "inria_wbc/controllers/pos_tracker.hpp" +#include "inria_wbc/controllers/talos_pos_tracker.hpp" + +#include "inria_wbc/exceptions.hpp" +#include "inria_wbc/robot_dart/cmd.hpp" +#include "inria_wbc/robot_dart/external_collision_detector.hpp" +#include "inria_wbc/robot_dart/self_collision_detector.hpp" +#include "inria_wbc/robot_dart/utils.hpp" +#include "inria_wbc/utils/timer.hpp" + +#include "utest.hpp" + +// parameters of the test suite +// could be tuned as arguments of the test suite +namespace cst { + static double sim_dt = 0.001; + static double duration = 2; // length of each test + static double ctrl_frequency = 1000; // 1000 Hz +} // namespace cst + +namespace y = YAML; + +// this is the only part that is robot specific +inria_wbc::controllers::SensorData sensor_data_franka(const std::shared_ptr& robot, +std::shared_ptr& controller) +{ + robot->fix_to_world(); // this is robot-specific too + + inria_wbc::controllers::SensorData sensor_data; + // left foot + sensor_data["lf_torque"] = Eigen::Vector3d::Zero(); + sensor_data["lf_force"] = Eigen::Vector3d::Zero(); + // right foot + sensor_data["rf_torque"] = Eigen::Vector3d::Zero(); + sensor_data["rf_force"] = Eigen::Vector3d::Zero(); + // accelerometer + sensor_data["acceleration"] = Eigen::Vector3d::Zero(); + sensor_data["velocity"] = Eigen::Vector3d::Zero(); + // joint positions (excluding floating base) + int ncontrollable = controller->controllable_dofs().size(); + sensor_data["positions"] = robot->skeleton()->getPositions().tail(ncontrollable); + return sensor_data; +} + +// this is the only part that is robot specific +inria_wbc::controllers::SensorData sensor_data_talos(const std::shared_ptr& r, std::shared_ptr& c) +{ + auto controller = std::dynamic_pointer_cast(c); + assert(controller); + auto robot = std::dynamic_pointer_cast(r); + assert(robot); + + inria_wbc::controllers::SensorData sensor_data; + + // update the sensors from the simulator + Eigen::VectorXd torques = Eigen::VectorXd::Zero(controller->torque_sensor_joints().size()); + for (size_t i = 0; i < controller->torque_sensor_joints().size(); ++i) + torques[i] = robot->torques().at(controller->torque_sensor_joints()[i])->torques()(0, 0); + sensor_data["joints_torque"] = torques; + sensor_data["lf_torque"] = robot->ft_foot_left().torque(); + sensor_data["lf_force"] = robot->ft_foot_left().force(); + sensor_data["rf_torque"] = robot->ft_foot_right().torque(); + sensor_data["rf_force"] = robot->ft_foot_right().force(); + sensor_data["velocity"] = robot->com_velocity().tail<3>(); + sensor_data["positions"] = robot->positions(controller->controllable_dofs(false)); + sensor_data["joint_velocities"] = robot->velocities(controller->controllable_dofs(false)); + sensor_data["floating_base_position"] = inria_wbc::robot_dart::floating_base_pos(robot->positions()); + sensor_data["floating_base_velocity"] = inria_wbc::robot_dart::floating_base_vel(robot->velocities()); + sensor_data["imu_pos"] = robot->imu().angular_position_vec(); + sensor_data["imu_vel"] = robot->imu().angular_velocity(); + sensor_data["imu_acc"] = robot->imu().linear_acceleration(); + + return sensor_data; +} + + +// this is the only part that is robot specific +inria_wbc::controllers::SensorData sensor_data_icub(const std::shared_ptr& r, std::shared_ptr& c) +{ + auto controller = std::dynamic_pointer_cast(c); + assert(controller); + auto robot = std::dynamic_pointer_cast(r); + assert(robot); + + inria_wbc::controllers::SensorData sensor_data; + + sensor_data["lf_torque"] = robot->ft_foot_left().torque(); + sensor_data["lf_force"] = robot->ft_foot_left().force(); + sensor_data["rf_torque"] = robot->ft_foot_right().torque(); + sensor_data["rf_force"] = robot->ft_foot_right().force(); + sensor_data["velocity"] = robot->com_velocity().tail<3>(); + sensor_data["positions"] = robot->positions(controller->controllable_dofs(false)); + sensor_data["joint_velocities"] = robot->velocities(controller->controllable_dofs(false)); + sensor_data["floating_base_position"] = inria_wbc::robot_dart::floating_base_pos(robot->positions()); + sensor_data["floating_base_velocity"] = inria_wbc::robot_dart::floating_base_vel(robot->velocities()); + sensor_data["imu_pos"] = robot->imu().angular_position_vec(); + sensor_data["imu_vel"] = robot->imu().angular_velocity(); + sensor_data["imu_acc"] = robot->imu().linear_acceleration(); + + return sensor_data; +} + +template +void test_behavior(utest::test_t test, + const std::string& base_path, + const std::string& controller_path, + const std::string& behavior_path, + const std::shared_ptr& robot, + const FData& fdata, + bool verbose = false) +{ + try { + inria_wbc::utils::Timer timer; + + // simulator + robot->set_actuator_types("servo"); // this one should never fail + robot->set_position_enforced(true); + + robot_dart::RobotDARTSimu simu(cst::sim_dt); + simu.set_collision_detector("fcl"); // this is the most generic one + simu.set_control_freq(cst::ctrl_frequency); + simu.add_robot(robot); + simu.add_checkerboard_floor(); + + // ----------------------- init ----------------------- + y::Node c_config = IWBC_CHECK(y::LoadFile(base_path + controller_path)); + c_config["CONTROLLER"]["base_path"] = base_path; + c_config["CONTROLLER"]["urdf"] = robot->model_filename(); + c_config["CONTROLLER"]["mimic_dof_names"] = robot->mimic_dof_names(); + c_config["CONTROLLER"]["verbose"] = verbose; + + // get the controller + auto controller_name = IWBC_CHECK(c_config["CONTROLLER"]["name"].as()); + auto controller = inria_wbc::controllers::Factory::instance().create(controller_name, c_config); + UTEST_CHECK(test, controller.get() != 0); + auto p_controller = std::dynamic_pointer_cast(controller); + UTEST_CHECK(test, p_controller.get() != 0); + UTEST_CHECK(test, !p_controller->tasks().empty()); + + // get the behavior (trajectories) + y::Node b_config = IWBC_CHECK(y::LoadFile(base_path + behavior_path)); + auto behavior_name = IWBC_CHECK(b_config["BEHAVIOR"]["name"].as()); + auto behavior = inria_wbc::behaviors::Factory::instance().create(behavior_name, controller, b_config); + UTEST_CHECK(test, behavior.get() != 0); + + // a few useful variables + auto controllable_dofs = controller->controllable_dofs(); + uint ncontrollable = controllable_dofs.size(); + + // init the robot + UTEST_CHECK(test, controller->q0().size() == controller->all_dofs().size()); + robot->set_positions(controller->q0(), controller->all_dofs()); + + // ----------------------- the main loop ----------------------- + using namespace std::chrono; + inria_wbc::controllers::SensorData sensor_data; + + Eigen::VectorXd cmd; + + while (simu.scheduler().next_time() < cst::duration) { + // update the sensors + sensor_data = fdata(robot, controller); + + // command + if (simu.schedule(simu.control_freq())) { + timer.begin("controller"); + behavior->update(sensor_data); + timer.end("controller"); + auto q = controller->q(false); + cmd = inria_wbc::robot_dart::compute_velocities(robot, q, cst::sim_dt, controller->all_dofs(false)); + if (simu.scheduler().current_time() == 0) + UTEST_CHECK(test, controller->filter_cmd(cmd).tail(ncontrollable).size() == controllable_dofs.size()); + robot->set_commands(controller->filter_cmd(cmd).tail(ncontrollable), controllable_dofs); + } + + // step the simulation + { + simu.step_world(); + } + } + auto t = timer["controller"]; + UTEST_INFO(test, "solver + behavior:" + std::to_string(t.time / t.iterations) + " " + "[" + std::to_string(t.min_time) + "," + std::to_string(t.max_time) + "]"); + UTEST_INFO(test, "test done"); + } + catch (std::exception& e) { + UTEST_ERROR(test, std::string("error in ref comparison:") + e.what()); + } +} + +int main(int argc, char** argv) +{ + + ////////////////// program options + namespace po = boost::program_options; + po::options_description desc("Test_robot options"); + // clang-format off + desc.add_options() + ("n_threads,n", po::value()->default_value(-1), "run tests in parallel (default = number of cores)") + ("single,s", po::value >()->multitoken(), "run a single test, args: robot_name controller_path behavior_path actuators") + ; + // clang-format on + po::variables_map vm; + try { + po::store(po::parse_command_line(argc, argv, desc), vm); + po::notify(vm); + } + catch (po::too_many_positional_options_error& e) { + // A positional argument like `opt2=option_value_2` was given + std::cerr << e.what() << std::endl; + std::cerr << desc << std::endl; + return 1; + } + catch (po::error_with_option_name& e) { + // Another usage error occurred + std::cerr << e.what() << std::endl; + std::cerr << desc << std::endl; + return 1; + } + + int n_threads = vm["n_threads"].as(); + + utest::TestSuite test_suite; + // paths are relative to the "tests" directory (to use make check) + + std::string path = "../../etc/"; + + /////////// Franka robot + { + std::string base_path = path + "/franka/"; + std::string name = "franka"; + std::string controller_path = "pos_tracker.yaml"; + auto behaviors = {"cartesian_line.yaml"}; + for (auto& behavior_path : behaviors) { + auto test1 = utest::make_test("[" + name + "] " + behavior_path); + auto franka = std::make_shared(); + UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, franka, sensor_data_franka, false)); + } + } + /////////// Talos robot + { + std::string base_path = path + "/talos/"; + std::string controller_path = "talos_pos_tracker.yaml"; + auto behaviors = { "arm.yaml", + "clapping.yaml", + "squat.yaml", + "walk.yaml", + "walk_on_spot.yaml", + "traj_teleop1.yaml"}; + std::string name = "talos"; + for (auto& behavior_path : behaviors) { + auto test1 = utest::make_test("[" + name + "] " + behavior_path); + auto talos = std::make_shared(); + UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, talos, sensor_data_talos, false)); + } + } + + /////////// Icub robot + { + std::string base_path = path + "/icub/"; + std::string controller_path = "humanoid_pos_tracker.yaml"; + auto behaviors = {"arm.yaml", + "squat.yaml", + "walk.yaml", + "walk_on_spot.yaml", + "traj_teleop1.yaml"}; + std::string name = "icub"; + for (auto& behavior_path : behaviors) { + auto test1 = utest::make_test("[" + name + "] " + behavior_path); + auto icub = std::make_shared(); + UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, icub, sensor_data_icub, false)); + } + } + ///////// RUN everything + test_suite.run(n_threads); + utest::write_report(test_suite, std::cout, true); + std::cout << "------------ SUMMARY ------------" << std::endl; + utest::write_report(test_suite, std::cout, false); + + if (test_suite.success()) + return 0; + else + return 1; + return 0; +} From 4fe858b29ec9a0a5238478f8a1e4c42da189dc2a Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Mon, 30 May 2022 10:57:34 +0200 Subject: [PATCH 29/46] remove old tests --- tests/ref_test_franka.yaml | 39 - tests/ref_test_talos.yaml | 2210 ------------------------------------ tests/test_all_robots.cpp | 3 +- tests/test_franka.cpp | 457 -------- tests/test_talos.cpp | 593 ---------- 5 files changed, 1 insertion(+), 3301 deletions(-) delete mode 100644 tests/ref_test_franka.yaml delete mode 100644 tests/ref_test_talos.yaml delete mode 100644 tests/test_franka.cpp delete mode 100644 tests/test_talos.cpp diff --git a/tests/ref_test_franka.yaml b/tests/ref_test_franka.yaml deleted file mode 100644 index 9b10c62a..00000000 --- a/tests/ref_test_franka.yaml +++ /dev/null @@ -1,39 +0,0 @@ -timestamp: 2021-06-29_20-35-51 -../../etc/franka/pos_tracker.yaml: - ../../etc/franka/cartesian_line.yaml: - servo: - fcl: - franka/franka.urdf: - time: - - 2.1072739999999999 - - 0.21072740000000001 - - 0.2008625 - - 0.0098503000000000011 - - 1.4600000000000001e-05 - ee: - - 0.090085309993550272 - - 0.089935963435977151 - velocity: - fcl: - franka/franka.urdf: - time: - - 1.862325 - - 0.18623250000000002 - - 0.17664740000000001 - - 0.0095706000000000003 - - 1.45e-05 - ee: - - 0.090085309993550272 - - 0.08993596343598162 - spd: - fcl: - franka/franka.urdf: - time: - - 2.2355170000000002 - - 0.22355170000000002 - - 0.18587480000000001 - - 0.0102925 - - 0.0273844 - ee: - - 0.090085309993550272 - - 0.091376721379293563 \ No newline at end of file diff --git a/tests/ref_test_talos.yaml b/tests/ref_test_talos.yaml deleted file mode 100644 index f6e43301..00000000 --- a/tests/ref_test_talos.yaml +++ /dev/null @@ -1,2210 +0,0 @@ -timestamp: 2021-06-08_17-21-11 -../../etc/talos/talos_pos_tracker.yaml: - ../../etc/talos/arm.yaml: - servo: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 10.926392 - - 1.0926392 - - 0.6587811 - - 0.4329102 - - 0.0009479 - com: - - 0.002985503011468953 - - 0.009669944999995584 - torso: - - 0 - - 0 - lf: - - 3.451342634984945e-07 - - 0.004935718715785986 - rf: - - 3.730596890907365e-07 - - 0.00509004387802613 - lh: - - 0.06227435752869405 - - 0.06694051035505597 - rh: - - 0.03869066250283776 - - 0.03694944931925605 - talos/talos.urdf: - time: - - 18.924668 - - 1.8924668 - - 1.205908 - - 0.6854226999999999 - - 0.0011361 - com: - - 0.003810292943635487 - - 0.01354942684746751 - rh: - - 0.03737637484083883 - - 0.04153289169605924 - rf: - - 6.915235575299666e-07 - - 0.007398567092505336 - lf: - - 5.590281994330906e-07 - - 0.00685195198913926 - lh: - - 0.0588062528201839 - - 0.06407851532214623 - torso: - - 0 - - 0 - stabilized_false: - talos/talos_fast.urdf: - time: - - 12.785228 - - 1.2785228 - - 0.9411929999999999 - - 0.3363275 - - 0.0010023 - com: - - 0.0008448881562454354 - - 0.003515908089006447 - lh: - - 0.05508793575199774 - - 0.05672586323295247 - rf: - - 1.11344529541743e-07 - - 0.004327010719684264 - lf: - - 1.045969637516711e-07 - - 0.004327071021469138 - torso: - - 0 - - 0 - rh: - - 0.03663349107743772 - - 0.03686698597846806 - talos/talos.urdf: - time: - - 16.902291 - - 1.6902291 - - 1.1981413 - - 0.4910684 - - 0.0010194 - com: - - 0.0008448875719054929 - - 0.006044396252166485 - torso: - - 0 - - 0 - lf: - - 1.045969642126192e-07 - - 0.006911752109580105 - lh: - - 0.05508793355434871 - - 0.05809723219274578 - rh: - - 0.03663349022981161 - - 0.03755884452298813 - rf: - - 1.113445032936488e-07 - - 0.006912015222085774 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 11.866126 - - 1.1866126 - - 0.6625253 - - 0.5229571999999999 - - 0.0011301 - com: - - 0.003050925334603784 - - 0.01026636820444153 - rf: - - 3.838355712805769e-07 - - 0.005138972407332361 - rh: - - 0.0390799667334626 - - 0.03380367139499522 - lf: - - 3.473921249123271e-07 - - 0.006504692956996943 - lh: - - 0.06273726615975389 - - 0.0734587049837739 - torso: - - 0 - - 0 - stabilized_false: - talos/talos_fast.urdf: - time: - - 11.32264 - - 1.132264 - - 0.7981501 - - 0.3334459 - - 0.0006680000000000001 - com: - - 0.0008448881562454359 - - 0.00354414330199741 - lh: - - 0.05508793575199777 - - 0.05670912234450171 - rh: - - 0.03663349107743774 - - 0.0368613563495485 - lf: - - 1.045969637511478e-07 - - 0.004347963931969177 - rf: - - 1.113445295433174e-07 - - 0.004348537340876284 - torso: - - 0 - - 0 - torque: - fcl: - stabilized_false: - talos/talos_fast.urdf: - time: - - 9.857511000000001 - - 0.9857510999999999 - - 0.6523732 - - 0.333376 - - 1.9e-06 - com: - - 0.00250726950443533 - - 0.002507476066544449 - torso: - - 0 - - 0 - rh: - - 0.01594717613638042 - - 0.01594877287587817 - lh: - - 0.05290896410789053 - - 0.05290723687877853 - rf: - - 0.00577640932632404 - - 0.005776987522057435 - lf: - - 0.005781965481101364 - - 0.005782544240355471 - talos/talos.urdf: - time: - - 18.322153 - - 1.8322153 - - 1.2978405 - - 0.5343696 - - 5.199999999999999e-06 - com: - - 0.00303789661980875 - - 0.003038161447530844 - torso: - - 0 - - 0 - rh: - - 0.01556366787617558 - - 0.01556523634591369 - lf: - - 0.009702358856136623 - - 0.009703336085574282 - lh: - - 0.05224098911827236 - - 0.05223873360721272 - rf: - - 0.009652537944146103 - - 0.009653504700585132 - dart: - stabilized_false: - talos/talos_fast.urdf: - time: - - 7.842667 - - 0.7842667 - - 0.4599448 - - 0.3243084 - - 1.35e-05 - com: - - 0.002448332875721597 - - 0.002448535441115542 - lh: - - 0.05085690090692327 - - 0.05085359856799045 - rh: - - 0.01249504398839575 - - 0.0124962781586615 - lf: - - 0.005770201150948618 - - 0.00577077862323826 - rf: - - 0.005770049088127571 - - 0.005770626558673103 - torso: - - 0 - - 0 - velocity: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 6.156072 - - 0.6156072 - - 0.1735358 - - 0.4412472 - - 0.0008242000000000001 - com: - - 0.0028506659365689 - - 0.008608776725245284 - lf: - - 3.385174064135407e-07 - - 0.004811764728767817 - torso: - - 0 - - 0 - lh: - - 0.06212537254777509 - - 0.06856035512103227 - rf: - - 3.617893284281237e-07 - - 0.00441070611967819 - rh: - - 0.03852050548532322 - - 0.03456351423659538 - talos/talos.urdf: - time: - - 12.947058 - - 1.2947058 - - 0.5673011 - - 0.7261775 - - 0.0012272 - com: - - 0.003895997197900436 - - 0.01335819256908329 - torso: - - 0 - - 0 - lf: - - 5.482845843299982e-07 - - 0.00713551562309443 - rf: - - 6.741321237119499e-07 - - 0.006971910570101195 - lh: - - 0.05913440374411275 - - 0.06993532243456821 - rh: - - 0.03764654852121409 - - 0.03607163089030288 - stabilized_false: - talos/talos_fast.urdf: - time: - - 5.962453 - - 0.5962453000000001 - - 0.2540196 - - 0.3415188 - - 0.0007069 - com: - - 0.0008448881562454349 - - 0.003424227653626272 - rf: - - 1.113445295417109e-07 - - 0.004262143354428101 - lh: - - 0.05508793575199777 - - 0.05681491698974282 - lf: - - 1.045969637568285e-07 - - 0.0042621459888542 - rh: - - 0.03663349107743773 - - 0.03703028370141269 - torso: - - 0 - - 0 - talos/talos.urdf: - time: - - 10.021442 - - 1.0021442 - - 0.5073054 - - 0.4937887 - - 0.0010501 - com: - - 0.0008448875719054931 - - 0.006153934144705627 - lf: - - 1.04596964214311e-07 - - 0.006897169187470772 - torso: - - 0 - - 0 - lh: - - 0.0550879335543487 - - 0.0582198252261532 - rf: - - 1.113445032923699e-07 - - 0.006897123140104025 - rh: - - 0.0366334902298116 - - 0.03773520154808383 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 5.734485 - - 0.5734485 - - 0.1213759 - - 0.4512219 - - 0.0008507 - com: - - 0.002980777343979729 - - 0.009073605457082428 - rh: - - 0.03847761802103244 - - 0.03582702640212853 - torso: - - 0 - - 0 - lh: - - 0.06196690537519684 - - 0.06849509364843392 - lf: - - 3.089884444397559e-07 - - 0.004602873667036036 - rf: - - 3.269542479154734e-07 - - 0.004584859538286004 - stabilized_false: - talos/talos_fast.urdf: - time: - - 7.070386 - - 0.7070386 - - 0.2370523 - - 0.4690479 - - 0.0009384 - com: - - 0.000844888156245435 - - 0.003424356113243014 - lf: - - 1.045969637522704e-07 - - 0.004262392942957154 - torso: - - 0 - - 0 - rf: - - 1.113445295430449e-07 - - 0.004262383851807211 - lh: - - 0.05508793575199776 - - 0.05681472532337573 - rh: - - 0.03663349107743774 - - 0.03703086855986866 - spd: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 12.410544 - - 1.2410544 - - 0.6245325 - - 0.4027724 - - 0.2137495 - com: - - 0.004925201210487374 - - 0.01410203107301986 - lf: - - 8.564733208344087e-07 - - 0.004231465999861943 - rf: - - 9.81487378093407e-07 - - 0.005056481204644642 - rh: - - 0.03844506772958974 - - 0.04031973744717103 - torso: - - 0 - - 0 - lh: - - 0.0549709809661837 - - 0.0600658833576795 - talos/talos.urdf: - time: - - 20.773559 - - 2.0773559 - - 1.0835646 - - 0.6528351 - - 0.3409562 - com: - - 0.007686268277128238 - - 0.02806680226161054 - lh: - - 0.05929523032314351 - - 0.07605162294514507 - lf: - - 1.088903234974134e-06 - - 0.006632137623147981 - rf: - - 1.502574910237438e-06 - - 0.007456384753070646 - torso: - - 0 - - 0 - rh: - - 0.03957286612773653 - - 0.04894363127351612 - stabilized_false: - talos/talos_fast.urdf: - time: - - 12.403468 - - 1.2403468 - - 0.6741860000000001 - - 0.3512507 - - 0.2149101 - com: - - 0.0008448881562454359 - - 0.004534650284172223 - rh: - - 0.03663349107743774 - - 0.03626163341687263 - lf: - - 1.045969637532305e-07 - - 0.00425560809060027 - rf: - - 1.113445295407634e-07 - - 0.00425660964136372 - lh: - - 0.05508793575199777 - - 0.05639049462923935 - torso: - - 0 - - 0 - talos/talos.urdf: - time: - - 20.860372 - - 2.0860372 - - 1.1923499 - - 0.5540062 - - 0.3396811 - com: - - 0.0008448875719054932 - - 0.006678699689986008 - rh: - - 0.03663349022981161 - - 0.03684653232899462 - lh: - - 0.0550879335543487 - - 0.05773549290476167 - torso: - - 0 - - 0 - lf: - - 1.04596964211751e-07 - - 0.006891412623998453 - rf: - - 1.113445032924816e-07 - - 0.006895938066336382 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 10.726308 - - 1.0726308 - - 0.4417048 - - 0.4157666 - - 0.2151594 - com: - - 0.004910531344566964 - - 0.01433881714238951 - rf: - - 9.586846894421964e-07 - - 0.00504308547218566 - torso: - - 0 - - 0 - rh: - - 0.03965926135613055 - - 0.04183028631541523 - lf: - - 8.412710052194107e-07 - - 0.004170878024393382 - lh: - - 0.05540661201009196 - - 0.06063903799655038 - stabilized_false: - talos/talos_fast.urdf: - time: - - 12.170209 - - 1.2170209 - - 0.5543455 - - 0.4101706 - - 0.2525048 - com: - - 0.0008448881562454349 - - 0.004477894413599565 - torso: - - 0 - - 0 - rh: - - 0.03663349107743772 - - 0.03628717065977668 - rf: - - 1.113445295434337e-07 - - 0.004262440066323083 - lf: - - 1.045969637512582e-07 - - 0.004262443486068422 - lh: - - 0.05508793575199774 - - 0.05642294234413383 - ../../etc/talos/squat.yaml: - servo: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 11.089464 - - 1.1089464 - - 0.6753711 - - 0.4325563 - - 0.001019 - com: - - 0.02603625518345671 - - 0.02668022728482573 - lf: - - 6.54656558493623e-06 - - 0.004357744138825244 - rh: - - 0.07697601412104454 - - 0.09034546012098413 - lh: - - 0.09810348370009424 - - 0.1001406125454447 - torso: - - 0 - - 0 - rf: - - 6.907255198891746e-06 - - 0.009570794648259339 - talos/talos.urdf: - time: - - 22.15717 - - 2.215717 - - 1.4451477 - - 0.7690189000000001 - - 0.0015504 - com: - - 0.02649117377765551 - - 0.03491418740351699 - rh: - - 0.08501247342265425 - - 0.1195739425043922 - rf: - - 7.222736977315817e-06 - - 0.01588941089887867 - lh: - - 0.08951555250714743 - - 0.1059375092053246 - torso: - - 0 - - 0 - lf: - - 6.669644018405254e-06 - - 0.008269255192252994 - stabilized_false: - talos/talos_fast.urdf: - time: - - 15.759443 - - 1.5759443 - - 1.1712736 - - 0.4035433 - - 0.0011274 - com: - - 0.025915618530645 - - 0.02458375631456412 - lh: - - 0.08563340331959224 - - 0.08859676250694912 - lf: - - 6.40080622247398e-06 - - 0.004324542485209833 - rf: - - 6.400107942244197e-06 - - 0.004324508703942524 - rh: - - 0.08562138441948589 - - 0.08857734229063312 - torso: - - 0 - - 0 - talos/talos.urdf: - time: - - 21.205724 - - 2.1205724 - - 1.5596408 - - 0.5597074 - - 0.0012242 - com: - - 0.02591562129704163 - - 0.02396324849180183 - lh: - - 0.08563334723576684 - - 0.09073463063313852 - lf: - - 6.400798398863034e-06 - - 0.006911700755252483 - rf: - - 6.400099965623717e-06 - - 0.006911843612569518 - rh: - - 0.08562133783340015 - - 0.09073212577693586 - torso: - - 0 - - 0 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 11.859453 - - 1.1859453 - - 0.6662880999999999 - - 0.5186199 - - 0.0010373 - com: - - 0.02595330414436534 - - 0.02720074927678184 - rh: - - 0.07585942631179973 - - 0.093382013371605 - lf: - - 6.536353327431566e-06 - - 0.004468985822213593 - torso: - - 0 - - 0 - rf: - - 6.80463637577378e-06 - - 0.01099577673048989 - lh: - - 0.09858955219771455 - - 0.1012289725317247 - stabilized_false: - talos/talos_fast.urdf: - time: - - 11.108393 - - 1.1108393 - - 0.7714470999999999 - - 0.3386673 - - 0.0007249 - com: - - 0.02591561853064499 - - 0.02459745426425376 - lh: - - 0.08563340331959224 - - 0.08859041221826071 - torso: - - 0 - - 0 - rh: - - 0.08562138441948589 - - 0.08858192163134306 - rf: - - 6.400107942252965e-06 - - 0.004344862382143084 - lf: - - 6.400806222483506e-06 - - 0.00434429384958008 - torque: - fcl: - stabilized_false: - talos/talos_fast.urdf: - time: - - 11.665668 - - 1.1665668 - - 0.7840467999999999 - - 0.3825175 - - 2.5e-06 - com: - - 0.02689713432818929 - - 0.0268419561469295 - rh: - - 0.09479966991109166 - - 0.09481583514346888 - lh: - - 0.0947241982176683 - - 0.09474035307095295 - lf: - - 0.005954960298141565 - - 0.005955579474611103 - rf: - - 0.005825218458365589 - - 0.005825805678620061 - torso: - - 0 - - 0 - talos/talos.urdf: - time: - - 24.864103 - - 2.4864103 - - 1.817061 - - 0.669314 - - 3.53e-05 - com: - - 0.02712503430178653 - - 0.02707002555737526 - lh: - - 0.09480302024625614 - - 0.09481922200314478 - torso: - - 0 - - 0 - rf: - - 0.00981370984411946 - - 0.009814710184028363 - rh: - - 0.09485139164741078 - - 0.09486759827055777 - lf: - - 0.009848743294572847 - - 0.009849748030960042 - dart: - stabilized_false: - talos/talos_fast.urdf: - time: - - 9.366540000000001 - - 0.936654 - - 0.5420589 - - 0.3945943 - - 8.000000000000001e-07 - com: - - 0.02680149318936405 - - 0.02674646921191727 - torso: - - 0 - - 0 - rf: - - 0.005819313878052496 - - 0.00581990230655201 - lf: - - 0.005817612765276271 - - 0.005818200982410368 - lh: - - 0.09423482680850401 - - 0.09425102119566094 - rh: - - 0.0942902444110808 - - 0.0943064419812128 - velocity: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 10.881073 - - 1.0881073 - - 0.3220946 - - 0.7641064 - - 0.0019063 - com: - - 0.02583215352427469 - - 0.02621371667014048 - lf: - - 6.533279152266874e-06 - - 0.004241844905060786 - torso: - - 0 - - 0 - rh: - - 0.07751869848123713 - - 0.09213340726341937 - rf: - - 6.842437110799919e-06 - - 0.009784050218984444 - lh: - - 0.09717048461647423 - - 0.09896715108382012 - talos/talos.urdf: - time: - - 13.172369 - - 1.3172369 - - 0.6216264 - - 0.6941803999999999 - - 0.0014301 - com: - - 0.02635821276148675 - - 0.02585973082293979 - rh: - - 0.08657712015419948 - - 0.09646470753354765 - torso: - - 0 - - 0 - rf: - - 7.28865149920225e-06 - - 0.007507811027119398 - lf: - - 6.617176789673995e-06 - - 0.007350432794579741 - lh: - - 0.08977915858205265 - - 0.09514091600428314 - stabilized_false: - talos/talos_fast.urdf: - time: - - 8.204091 - - 0.8204091 - - 0.4198019 - - 0.3995289 - - 0.0010783 - com: - - 0.02591561853064499 - - 0.02452895711562096 - lf: - - 6.400806222487276e-06 - - 0.004265810962802683 - rh: - - 0.08562138441948591 - - 0.08859387255463927 - rf: - - 6.400107942263097e-06 - - 0.004265813513507779 - lh: - - 0.08563340331959227 - - 0.08860776542475776 - torso: - - 0 - - 0 - talos/talos.urdf: - time: - - 21.794989 - - 2.1794989 - - 1.2241678 - - 0.9524637 - - 0.0028674 - com: - - 0.02591562129704163 - - 0.02400993005517664 - rf: - - 6.400099965584843e-06 - - 0.006902007655654174 - rh: - - 0.0856213378334001 - - 0.09075181529892855 - torso: - - 0 - - 0 - lh: - - 0.08563334723576688 - - 0.09076635475151683 - lf: - - 6.400798398827202e-06 - - 0.006901993485172076 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 7.026258 - - 0.7026258 - - 0.149881 - - 0.5516454 - - 0.0010994 - com: - - 0.0258765752971102 - - 0.02651642287382111 - lh: - - 0.09779399494173364 - - 0.1012438694486017 - rf: - - 6.788721193076114e-06 - - 0.01132265061666946 - torso: - - 0 - - 0 - lf: - - 6.542625343098154e-06 - - 0.004343487790946144 - rh: - - 0.0764838191049742 - - 0.09728915506737069 - stabilized_false: - talos/talos_fast.urdf: - time: - - 6.767845 - - 0.6767844999999999 - - 0.2588495 - - 0.4168261 - - 0.0011089 - com: - - 0.02591561853064498 - - 0.02452957925688298 - rf: - - 6.400107942257262e-06 - - 0.004265873454604853 - torso: - - 0 - - 0 - rh: - - 0.08562138441948593 - - 0.08859453669450078 - lh: - - 0.08563340331959224 - - 0.08860774349405159 - lf: - - 6.400806222481919e-06 - - 0.004265882833855999 - spd: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 19.404414 - - 1.9404414 - - 0.9949736 - - 0.6215514 - - 0.3239164 - com: - - 0.02651983318677655 - - 0.02981877018804235 - lf: - - 7.093034715908445e-06 - - 0.004224081273709176 - torso: - - 0 - - 0 - rf: - - 7.67287816250234e-06 - - 0.005070117267948517 - rh: - - 0.09705724881487095 - - 0.1009961078563948 - lh: - - 0.08104381008707942 - - 0.08661345873487107 - talos/talos.urdf: - time: - - 31.650698 - - 3.1650698 - - 1.7027603 - - 0.9538139999999999 - - 0.5084955 - com: - - 0.02842548288797766 - - 0.03796773041819419 - rh: - - 0.0946839418773376 - - 0.103991230873733 - lf: - - 6.844152308847969e-06 - - 0.006756841426718377 - rf: - - 8.7271035423744e-06 - - 0.007610565313330335 - lh: - - 0.08797569821636413 - - 0.1029866730634057 - torso: - - 0 - - 0 - stabilized_false: - talos/talos_fast.urdf: - time: - - 15.080839 - - 1.5080839 - - 0.8276418999999999 - - 0.420344 - - 0.260098 - com: - - 0.025915618530645 - - 0.02568520027354013 - rf: - - 6.400107942240525e-06 - - 0.004289142822938811 - lh: - - 0.08563340331959222 - - 0.08837198765756241 - rh: - - 0.08562138441948594 - - 0.08845530207931347 - torso: - - 0 - - 0 - lf: - - 6.400806222465924e-06 - - 0.004283715317494431 - talos/talos.urdf: - time: - - 31.299945 - - 3.1299945 - - 1.7528131 - - 0.8460757 - - 0.5311056999999999 - com: - - 0.02591562129704163 - - 0.02499956802746272 - torso: - - 0 - - 0 - rf: - - 6.400099965584843e-06 - - 0.006921559123584326 - rh: - - 0.0856213378334001 - - 0.09051104795262904 - lh: - - 0.08563334723576688 - - 0.09049077914955318 - lf: - - 6.400798398827202e-06 - - 0.006924253110090849 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 10.834931 - - 1.0834931 - - 0.4430752 - - 0.419686 - - 0.2207319 - com: - - 0.02611961472747817 - - 0.02871609090693138 - lf: - - 6.992263752693751e-06 - - 0.004194803354219752 - lh: - - 0.07999647790659936 - - 0.08584718417973666 - rf: - - 7.255758088396113e-06 - - 0.005616634520309445 - torso: - - 0 - - 0 - rh: - - 0.09613570553132571 - - 0.09840872167395023 - stabilized_false: - talos/talos_fast.urdf: - time: - - 12.099209 - - 1.2099209 - - 0.5452777 - - 0.4096419 - - 0.2550013 - com: - - 0.025915618530645 - - 0.02565169029764669 - torso: - - 0 - - 0 - rh: - - 0.08562138441948597 - - 0.08841955694173496 - lh: - - 0.08563340331959224 - - 0.0884603126809799 - lf: - - 6.400806222477273e-06 - - 0.004284283936903456 - rf: - - 6.400107942250636e-06 - - 0.004280529451540854 - ../../etc/talos/clapping.yaml: - servo: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 11.441345 - - 1.1441345 - - 0.6870438 - - 0.456071 - - 0.0010197 - com: - - 0.004572001656411312 - - 0.02145697730417254 - lh: - - 0.4709350604711195 - - 0.4603099830694511 - rh: - - 0.4684555029697485 - - 0.4747454404458177 - lf: - - 5.8194533831115e-06 - - 0.007206210451737217 - rf: - - 5.390411152027137e-06 - - 0.02433694418466587 - torso: - - 0 - - 0 - talos/talos.urdf: - time: - - 21.221009 - - 2.1221009 - - 1.4555735 - - 0.6653839 - - 0.0011435 - com: - - 0.00562176607675308 - - 0.02432632234605406 - rh: - - 0.4674372190053797 - - 0.4661935437469822 - lh: - - 0.4692128461081041 - - 0.4613518424062645 - rf: - - 6.082260089340439e-06 - - 0.02031038986225425 - lf: - - 5.932026607075032e-06 - - 0.01037010985140326 - torso: - - 0 - - 0 - stabilized_false: - talos/talos_fast.urdf: - time: - - 13.418956 - - 1.3418956 - - 0.9884341 - - 0.3524464 - - 0.0010151 - com: - - 0.0012656282806764 - - 0.004813765618838784 - lf: - - 5.126452561933995e-06 - - 0.00432581072235642 - rf: - - 5.138190262177374e-06 - - 0.004326089481048003 - rh: - - 0.4688180231121187 - - 0.4688254112376064 - lh: - - 0.468863675173513 - - 0.4688659486091077 - torso: - - 0 - - 0 - talos/talos.urdf: - time: - - 21.137052 - - 2.1137052 - - 1.5669464 - - 0.5456378 - - 0.001121 - com: - - 0.00126562790420921 - - 0.007215985108036643 - torso: - - 0 - - 0 - rf: - - 5.138189580236856e-06 - - 0.006912915159383839 - lf: - - 5.126451889473778e-06 - - 0.006912679816599483 - lh: - - 0.4688636714024522 - - 0.4694991112672994 - rh: - - 0.4688180176750279 - - 0.4694786096065372 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 10.752111 - - 1.0752111 - - 0.5995951 - - 0.4745714 - - 0.0010446 - com: - - 0.004531564454084756 - - 0.02041276064500636 - rh: - - 0.468628130963275 - - 0.4660625206936298 - lf: - - 5.902056601041298e-06 - - 0.009983171687729669 - rf: - - 5.455005289026615e-06 - - 0.01998311080295983 - lh: - - 0.4710653003307035 - - 0.4657784347065131 - torso: - - 0 - - 0 - stabilized_false: - talos/talos_fast.urdf: - time: - - 13.107407 - - 1.3107407 - - 0.8940361 - - 0.4155183 - - 0.0011863 - com: - - 0.001265628280676401 - - 0.004851174356651966 - rf: - - 5.138190262179398e-06 - - 0.004347169729316247 - torso: - - 0 - - 0 - lf: - - 5.126452561932552e-06 - - 0.004346441520019898 - rh: - - 0.4688180231121185 - - 0.4687918292822155 - lh: - - 0.4688636751735128 - - 0.4688245345704697 - torque: - fcl: - stabilized_false: - talos/talos_fast.urdf: - time: - - 10.71363 - - 1.071363 - - 0.7100256 - - 0.3613327 - - 4.7e-06 - com: - - 0.002663695204736879 - - 0.002663941385775137 - lh: - - 0.4770692165870812 - - 0.4769627318899154 - rf: - - 0.005698917351083114 - - 0.005699495151342776 - lf: - - 0.005849637333755375 - - 0.005850231950415685 - torso: - - 0 - - 0 - rh: - - 0.4771168832660398 - - 0.4770108445870067 - talos/talos.urdf: - time: - - 19.897777 - - 1.9897777 - - 1.4227146 - - 0.5670614 - - 1.7e-06 - com: - - 0.003034632449109864 - - 0.003034929850447431 - rh: - - 0.4773952939138872 - - 0.4772900580087478 - lf: - - 0.01018594699974569 - - 0.0101869909972099 - lh: - - 0.4773889312682991 - - 0.4772832616752194 - rf: - - 0.009863311067738899 - - 0.009864314198871191 - torso: - - 0 - - 0 - dart: - stabilized_false: - talos/talos_fast.urdf: - time: - - 8.313817999999999 - - 0.8313817999999999 - - 0.4772522 - - 0.3541265 - - 3.1e-06 - com: - - 0.002702478089087276 - - 0.00270272727350917 - lf: - - 0.005805477353454054 - - 0.005806063065741966 - lh: - - 0.4774403509896593 - - 0.477335420122787 - torso: - - 0 - - 0 - rf: - - 0.005800698438489185 - - 0.005801281778361469 - rh: - - 0.4774788389726557 - - 0.4773744357396412 - velocity: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 6.419726 - - 0.6419726 - - 0.1872365 - - 0.4538224 - - 0.0009136999999999999 - com: - - 0.004128836893922625 - - 0.01632023078494737 - torso: - - 0 - - 0 - rh: - - 0.4681688383686665 - - 0.4705454677810414 - rf: - - 5.27521952120458e-06 - - 0.01807501975281775 - lf: - - 5.716812283538818e-06 - - 0.00562483259770737 - lh: - - 0.470772224896265 - - 0.4630753305356646 - talos/talos.urdf: - time: - - 14.963054 - - 1.4963054 - - 0.7211852 - - 0.7735148000000001 - - 0.0016054 - com: - - 0.00503293710943514 - - 0.02226121579466097 - torso: - - 0 - - 0 - lf: - - 5.725878803647497e-06 - - 0.008247261815129789 - rf: - - 5.852997238828936e-06 - - 0.02536115218930749 - lh: - - 0.4697374135309593 - - 0.459040644804083 - rh: - - 0.4679309144451499 - - 0.4768350077974985 - stabilized_false: - talos/talos_fast.urdf: - time: - - 7.571079 - - 0.7571079000000001 - - 0.3710579 - - 0.3849927 - - 0.0010573 - com: - - 0.00126562828067639 - - 0.004590672264493104 - torso: - - 0 - - 0 - rf: - - 5.138190262193147e-06 - - 0.004264590588857814 - lh: - - 0.4688636751735129 - - 0.4692099249619183 - lf: - - 5.126452561973632e-06 - - 0.004264640716708257 - rh: - - 0.4688180231121186 - - 0.4691718824419382 - talos/talos.urdf: - time: - - 13.532931 - - 1.3532931 - - 0.7690277 - - 0.5830808 - - 0.0011846 - com: - - 0.001265627904210275 - - 0.007204586874665214 - rf: - - 5.138189580325004e-06 - - 0.00690081633171449 - lh: - - 0.4688636714024501 - - 0.4698800606362569 - torso: - - 0 - - 0 - rh: - - 0.4688180176750221 - - 0.4698449968979596 - lf: - - 5.126451889548365e-06 - - 0.006900871549399406 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 5.929982 - - 0.5929982 - - 0.1265467 - - 0.465459 - - 0.0009925000000000001 - com: - - 0.00422641898693734 - - 0.0167083516292071 - rf: - - 5.370274402301447e-06 - - 0.01580085586348856 - lh: - - 0.4709144404367285 - - 0.4684863375524645 - torso: - - 0 - - 0 - lf: - - 5.769264523884982e-06 - - 0.009035704047825947 - rh: - - 0.4681939575403016 - - 0.4637865283574532 - stabilized_false: - talos/talos_fast.urdf: - time: - - 5.753138 - - 0.5753138 - - 0.2182092 - - 0.3562272 - - 0.0008774 - com: - - 0.001265628280676372 - - 0.004590847997524797 - lh: - - 0.4688636751735128 - - 0.4692100432189349 - torso: - - 0 - - 0 - lf: - - 5.126452561945336e-06 - - 0.004264667658812535 - rf: - - 5.138190262191823e-06 - - 0.004264674816965376 - rh: - - 0.4688180231121185 - - 0.4691714495919079 - spd: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 13.480534 - - 1.3480534 - - 0.6910485 - - 0.4289652 - - 0.2280397 - com: - - 0.005750893784302291 - - 0.01431759646486101 - lf: - - 5.952907024915978e-06 - - 0.004415598507991833 - torso: - - 0 - - 0 - rf: - - 6.571786823757113e-06 - - 0.006392753904737199 - lh: - - 0.4694785362754079 - - 0.471005461765828 - rh: - - 0.4685550338838046 - - 0.4691061709244843 - talos/talos.urdf: - time: - - 21.468819 - - 2.1468819 - - 1.1929746 - - 0.6217663 - - 0.332141 - com: - - 0.007620204598731938 - - 0.02240880250484528 - torso: - - 0 - - 0 - rh: - - 0.4678549826070937 - - 0.4708780562720783 - lh: - - 0.4696396206156927 - - 0.4704100630468354 - lf: - - 6.542269793357673e-06 - - 0.006807780765343399 - rf: - - 6.709564951611918e-06 - - 0.009318352427720399 - stabilized_false: - talos/talos_fast.urdf: - time: - - 14.643834 - - 1.4643834 - - 0.7981033 - - 0.4122521 - - 0.254028 - com: - - 0.001265628280676392 - - 0.005289282643547791 - rh: - - 0.4688180231121186 - - 0.4708253729980391 - lh: - - 0.4688636751735129 - - 0.4707368937909417 - torso: - - 0 - - 0 - rf: - - 5.138190262205886e-06 - - 0.004273031384770667 - lf: - - 5.12645256195738e-06 - - 0.004280478738546312 - talos/talos.urdf: - time: - - 19.301021 - - 1.9301021 - - 1.0926043 - - 0.5206829000000001 - - 0.3168149 - com: - - 0.001265627904210176 - - 0.007797266990811441 - lh: - - 0.4688636714024512 - - 0.4714897433972561 - rh: - - 0.4688180176750218 - - 0.4715084021831643 - rf: - - 5.138189580387738e-06 - - 0.006914103494865222 - lf: - - 5.126451889604243e-06 - - 0.006921006510248649 - torso: - - 0 - - 0 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 10.869587 - - 1.0869587 - - 0.4436288 - - 0.4253938 - - 0.2179361 - com: - - 0.005636076302177356 - - 0.01421398690663566 - lh: - - 0.4699592041361844 - - 0.4724767720010219 - rf: - - 6.431997594432319e-06 - - 0.006075703129431286 - torso: - - 0 - - 0 - lf: - - 5.817578063942935e-06 - - 0.004200547066885807 - rh: - - 0.4684607761663783 - - 0.4678809570315168 - stabilized_false: - talos/talos_fast.urdf: - time: - - 12.152011 - - 1.2152011 - - 0.5431217 - - 0.4174058 - - 0.2546736 - com: - - 0.001265628280676375 - - 0.005267185072081209 - lh: - - 0.4688636751735128 - - 0.4708224866685927 - rh: - - 0.4688180231121186 - - 0.4706997335886378 - torso: - - 0 - - 0 - lf: - - 5.126452561939802e-06 - - 0.004276880741324496 - rf: - - 5.138190262186111e-06 - - 0.004275265132859482 - ../../etc/talos/walk_on_spot.yaml: - servo: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 10.866643 - - 1.0866643 - - 0.7041475 - - 0.3814823 - - 0.0010345 - com: - - 0.008534387668197885 - - 0.009774551827752401 - lf: - - 0.004803672025968089 - - 0.009072142474914148 - rf: - - 2.864291525810545e-07 - - 0.004297990331787268 - torso: - - 0 - - 0 - rh: - - 0.08735265613021607 - - 0.09067275724942814 - lh: - - 0.02448716319781678 - - 0.02531904941429108 - talos/talos.urdf: - time: - - 18.906432 - - 1.8906432 - - 1.2938803 - - 0.5953478 - - 0.0014151 - com: - - 0.007269007029990246 - - 0.01153663688284126 - lh: - - 0.0238968745401361 - - 0.02330545742777654 - rh: - - 0.08459437740182793 - - 0.09000153029329148 - torso: - - 0 - - 0 - lf: - - 0.004623971464143389 - - 0.01166304549073277 - rf: - - 2.352527757094759e-07 - - 0.006953262384426551 - stabilized_false: - talos/talos_fast.urdf: - time: - - 11.65083 - - 1.165083 - - 0.8610248 - - 0.3030333 - - 0.0010249 - com: - - 0.004996213523062676 - - 0.005871349705252315 - rf: - - 1.460637910578153e-07 - - 0.004326997169143298 - lf: - - 0.004487483055897448 - - 0.008775639457159914 - lh: - - 0.02504131038541349 - - 0.02335557631171581 - torso: - - 0 - - 0 - rh: - - 0.08535060092597638 - - 0.08565480744728597 - talos/talos.urdf: - time: - - 15.646424 - - 1.5646424 - - 1.1323884 - - 0.4312325 - - 0.0010215 - com: - - 0.004996211562237993 - - 0.007836421259036592 - rf: - - 1.460639576543337e-07 - - 0.006915793370147816 - torso: - - 0 - - 0 - lf: - - 0.004487482788028217 - - 0.01141003670985036 - rh: - - 0.08535058901292453 - - 0.08615535565815996 - lh: - - 0.02504140589845444 - - 0.02191739819600309 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 10.673735 - - 1.0673735 - - 0.6439827 - - 0.4222909 - - 0.0010999 - com: - - 0.008657940925197418 - - 0.009772887126551565 - rh: - - 0.08751802845874247 - - 0.09038301053572395 - lh: - - 0.02449537659753708 - - 0.02432427791930323 - rf: - - 3.134345301656551e-07 - - 0.004401376547385948 - lf: - - 0.004808496375722258 - - 0.009035106759397572 - torso: - - 0 - - 0 - stabilized_false: - talos/talos_fast.urdf: - time: - - 10.381615 - - 1.0381615 - - 0.7420808 - - 0.2950942 - - 0.0009865 - com: - - 0.004996213523062678 - - 0.005891786886645969 - lf: - - 0.004487483055897445 - - 0.008790240087091035 - torso: - - 0 - - 0 - rh: - - 0.08535060092597636 - - 0.08563896846148725 - rf: - - 1.460637910580701e-07 - - 0.004348593794926126 - lh: - - 0.0250413103854135 - - 0.02340267542665041 - torque: - fcl: - stabilized_false: - talos/talos_fast.urdf: - time: - - 8.427108 - - 0.8427108 - - 0.5485726 - - 0.2941329 - - 5.3e-06 - com: - - 0.005784512451513985 - - 0.005776921600496568 - rf: - - 0.005778679338762189 - - 0.005779257826376686 - rh: - - 0.06476209481549106 - - 0.06477089935304826 - lf: - - 0.008551581692771615 - - 0.008542242993205088 - lh: - - 0.05834278937203884 - - 0.05835042031450666 - torso: - - 0 - - 0 - talos/talos.urdf: - time: - - 15.691099 - - 1.5691099 - - 1.1323978 - - 0.4367062 - - 5.9e-06 - com: - - 0.005961869318832037 - - 0.005954410565688637 - lh: - - 0.05836977628360967 - - 0.05837740577889381 - rf: - - 0.009732324550828016 - - 0.009733301135736734 - torso: - - 0 - - 0 - lf: - - 0.01053856839807319 - - 0.01052945476303082 - rh: - - 0.06504822830964524 - - 0.06505707026748991 - dart: - stabilized_false: - talos/talos_fast.urdf: - time: - - 9.224719 - - 0.9224719 - - 0.5369406 - - 0.3855225 - - 8.8e-06 - com: - - 0.005758512977204268 - - 0.005750842115436437 - torso: - - 0 - - 0 - lh: - - 0.05449821011654345 - - 0.05450559425536372 - lf: - - 0.008500750943816922 - - 0.008491409807844637 - rh: - - 0.06775880836416762 - - 0.06776804394513891 - rf: - - 0.005770227756856428 - - 0.005770805264663008 - velocity: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 6.077559 - - 0.6077559 - - 0.2010912 - - 0.4055457 - - 0.001119 - com: - - 0.007746628296752557 - - 0.008410687760884746 - torso: - - 0 - - 0 - rf: - - 2.098822204663065e-07 - - 0.004365466522768026 - rh: - - 0.0871547658328081 - - 0.09647891030868808 - lf: - - 0.004774956853201691 - - 0.0102418938732263 - lh: - - 0.02341717178606628 - - 0.02990827746695382 - talos/talos.urdf: - time: - - 9.639964000000001 - - 0.9639964000000001 - - 0.4687149 - - 0.4942542 - - 0.0010273 - com: - - 0.007310572441794684 - - 0.01171114373144085 - torso: - - 0 - - 0 - lf: - - 0.004621431390255558 - - 0.01177150012434067 - rf: - - 2.361765025249906e-07 - - 0.006781309524453692 - lh: - - 0.02349029494084407 - - 0.02131343767695818 - rh: - - 0.08445129910702734 - - 0.0879492303116287 - stabilized_false: - talos/talos_fast.urdf: - time: - - 5.226137 - - 0.5226137 - - 0.2322544 - - 0.2897865 - - 0.0005727999999999999 - com: - - 0.004996213523062677 - - 0.005796328647784502 - rh: - - 0.08535060092597635 - - 0.08590033047453564 - torso: - - 0 - - 0 - rf: - - 1.460637910597911e-07 - - 0.004262554752846611 - lh: - - 0.02504131038541349 - - 0.02290603029461833 - lf: - - 0.004487483055897449 - - 0.008730596620209915 - talos/talos.urdf: - time: - - 9.211437999999999 - - 0.9211438 - - 0.4816276 - - 0.4384745 - - 0.0010417 - com: - - 0.004996211562237987 - - 0.007925384581251303 - lh: - - 0.02504140589845445 - - 0.02141115669042481 - torso: - - 0 - - 0 - rf: - - 1.460639576586646e-07 - - 0.006898750406239421 - lf: - - 0.004487482788028216 - - 0.01139790949644323 - rh: - - 0.08535058901292451 - - 0.08641162316527161 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 6.857484 - - 0.6857484 - - 0.1630802 - - 0.5216167 - - 0.0010515 - com: - - 0.008793580832927731 - - 0.01025089153073152 - torso: - - 0 - - 0 - lh: - - 0.02503746857478702 - - 0.02372965469769582 - lf: - - 0.004822202959070493 - - 0.009106835473228804 - rh: - - 0.08737252118979949 - - 0.0898495216444427 - rf: - - 3.852276374405262e-07 - - 0.004290613464084729 - stabilized_false: - talos/talos_fast.urdf: - time: - - 4.54177 - - 0.4541770000000001 - - 0.1684722 - - 0.285487 - - 0.0002178 - com: - - 0.004996213523062675 - - 0.005796280730534027 - rf: - - 1.460637910572614e-07 - - 0.004262425171404663 - lh: - - 0.02504131038541349 - - 0.02290670328641051 - rh: - - 0.08535060092597635 - - 0.08590059854555004 - torso: - - 0 - - 0 - lf: - - 0.004487483055897448 - - 0.008730476683400282 - spd: - fcl: - stabilized_true: - talos/talos_fast.urdf: - time: - - 11.023628 - - 1.1023628 - - 0.5521931 - - 0.3355732 - - 0.2145965 - com: - - 0.007335917051654078 - - 0.01218541711851772 - torso: - - 0 - - 0 - rh: - - 0.08409286648476617 - - 0.08466008136772757 - rf: - - 3.540589302808406e-07 - - 0.004648433967904937 - lf: - - 0.004591532717969448 - - 0.01087212268014861 - lh: - - 0.02480291137922382 - - 0.02630615050418301 - talos/talos.urdf: - time: - - 18.384229 - - 1.8384229 - - 0.9951479000000001 - - 0.5213258 - - 0.3219492 - com: - - 0.007739180382338924 - - 0.01854846233206058 - lf: - - 0.004591355027139573 - - 0.01343910396699025 - lh: - - 0.02581020688503997 - - 0.03270461194560609 - rh: - - 0.0839652905360735 - - 0.08799287760802438 - torso: - - 0 - - 0 - rf: - - 3.204011808229208e-07 - - 0.007233979222723156 - stabilized_false: - talos/talos_fast.urdf: - time: - - 10.565367 - - 1.0565367 - - 0.5420106 - - 0.2987209 - - 0.2158052 - com: - - 0.004996213523062678 - - 0.007837273843102328 - lh: - - 0.02504131038541349 - - 0.02424177110128069 - lf: - - 0.004487483055897465 - - 0.01058285863498165 - torso: - - 0 - - 0 - rh: - - 0.08535060092597636 - - 0.08438679896777811 - rf: - - 1.460637910750659e-07 - - 0.004254749641903059 - talos/talos.urdf: - time: - - 19.292971 - - 1.9292971 - - 1.0904248 - - 0.4901397 - - 0.3487326 - com: - - 0.004996211562237987 - - 0.009222333632635216 - rh: - - 0.08535058901292451 - - 0.08516835974128496 - lf: - - 0.004487482788028218 - - 0.01317617965457914 - torso: - - 0 - - 0 - lh: - - 0.02504140589845445 - - 0.02317313691579419 - rf: - - 1.460639576588625e-07 - - 0.006888330655680986 - dart: - stabilized_true: - talos/talos_fast.urdf: - time: - - 9.602831 - - 0.9602831000000001 - - 0.4101227 - - 0.3353398 - - 0.2148206 - com: - - 0.007458149834081939 - - 0.0123521914392082 - torso: - - 0 - - 0 - rh: - - 0.08407904233195486 - - 0.08475901106258278 - lf: - - 0.00459736090160713 - - 0.01073254771521028 - lh: - - 0.02478573994857894 - - 0.02742579609940066 - rf: - - 3.591290158570297e-07 - - 0.00421401059754858 - stabilized_false: - talos/talos_fast.urdf: - time: - - 11.23872 - - 1.123872 - - 0.5081285 - - 0.3503797 - - 0.2653638 - com: - - 0.004996213523062677 - - 0.007746593796656727 - rh: - - 0.08535060092597635 - - 0.08443645990663576 - lf: - - 0.004487483055897447 - - 0.0105973466614804 - lh: - - 0.02504131038541349 - - 0.0242084785125759 - rf: - - 1.460637910604292e-07 - - 0.004262493981973313 - torso: - - 0 - - 0 \ No newline at end of file diff --git a/tests/test_all_robots.cpp b/tests/test_all_robots.cpp index bc0bf274..899c74e5 100644 --- a/tests/test_all_robots.cpp +++ b/tests/test_all_robots.cpp @@ -198,8 +198,7 @@ void test_behavior(utest::test_t test, } } auto t = timer["controller"]; - UTEST_INFO(test, "solver + behavior:" + std::to_string(t.time / t.iterations) + " " + "[" + std::to_string(t.min_time) + "," + std::to_string(t.max_time) + "]"); - UTEST_INFO(test, "test done"); + UTEST_INFO(test, "solver + behavior:" + std::to_string(t.time / t.iterations / 1000) + "ms " + "[" + std::to_string(t.min_time / 1000) + "," + std::to_string(t.max_time / 1000) + "]"); } catch (std::exception& e) { UTEST_ERROR(test, std::string("error in ref comparison:") + e.what()); diff --git a/tests/test_franka.cpp b/tests/test_franka.cpp deleted file mode 100644 index 267bdb6f..00000000 --- a/tests/test_franka.cpp +++ /dev/null @@ -1,457 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#ifdef GRAPHIC -#include -#endif - -#include "inria_wbc/behaviors/behavior.hpp" -#include "inria_wbc/robot_dart/cmd.hpp" - -#include "inria_wbc/behaviors/behavior.hpp" -#include "inria_wbc/controllers/pos_tracker.hpp" -#include "inria_wbc/exceptions.hpp" -#include "inria_wbc/robot_dart/cmd.hpp" -#include "inria_wbc/robot_dart/external_collision_detector.hpp" -#include "inria_wbc/robot_dart/self_collision_detector.hpp" -#include "inria_wbc/robot_dart/utils.hpp" - -#include "utest.hpp" - -namespace cst { - static constexpr double dt = 0.001; - static constexpr double duration = 10; - static constexpr double frequency = 1000; - static constexpr double tolerance = 5e-3; - static const std::string ref_path = "../../tests/ref_test_franka.yaml"; - static const std::string base_path = "../../etc/franka/"; - -} // namespace cst - -namespace inria_wbc { - namespace tests { - struct SE3TaskData { - std::string name; - std::string tracked; - std::string mask; - double weight; - double error_dart = 0.0; - double error_tsid = 0.0; - }; - } // namespace tests -} // namespace inria_wbc - -namespace y = YAML; - -// for timestamps -inline std::string date() -{ - char date[30]; - time_t date_time; - time(&date_time); - strftime(date, 30, "%Y-%m-%d_%H-%M-%S", localtime(&date_time)); - return std::string(date); -} - -// we parse again: -// - to check that the controller parses as expected -// - and to get additional info (mask, etc.) -std::vector parse_tasks(const y::Node& config) -{ - std::vector tasks; - auto c = IWBC_CHECK(config["CONTROLLER"]); - auto path = IWBC_CHECK(c["base_path"].as()); - auto task_file = IWBC_CHECK(c["tasks"].as()); - auto p = path + "/" + task_file; - auto task_list = IWBC_CHECK(y::LoadFile(p)); - for (auto it = task_list.begin(); it != task_list.end(); ++it) { - auto name = IWBC_CHECK(it->first.as()); - auto type = IWBC_CHECK(it->second["type"].as()); - if (type == "se3") { - auto weight = IWBC_CHECK(it->second["weight"].as()); - auto mask = IWBC_CHECK(it->second["mask"].as()); - auto tracked = IWBC_CHECK(it->second["tracked"].as()); - tasks.push_back({name, tracked, mask, weight}); - } - } - - // std::cout << "SE3 TASKS (frames): "; - // for (auto& x : tasks) - // std::cout << x.name << "[" << x.tracked << "] "; - // std::cout << std::endl; - - return tasks; -} - -void test_behavior(utest::test_t test, - const std::string& controller_path, - const std::string& behavior_path, - robot_dart::RobotDARTSimu& simu, - const std::shared_ptr& robot, - const std::string& actuator_type, - const y::Node& ref, - std::shared_ptr yout, - bool verbose = false) -{ - // ----------------------- init ----------------------- - - y::Node c_config = IWBC_CHECK(y::LoadFile(controller_path)); - c_config["CONTROLLER"]["base_path"] = cst::base_path; - c_config["CONTROLLER"]["urdf"] = robot->model_filename(); - c_config["CONTROLLER"]["mimic_dof_names"] = robot->mimic_dof_names(); - c_config["CONTROLLER"]["verbose"] = verbose; - - // get the controller - auto controller_name = IWBC_CHECK(c_config["CONTROLLER"]["name"].as()); - if (actuator_type == "torque") // force closed-loop - c_config["CONTROLLER"]["closed_loop"] = true; - auto controller = inria_wbc::controllers::Factory::instance().create(controller_name, c_config); - UTEST_CHECK(test, controller.get() != 0); - auto p_controller = std::dynamic_pointer_cast(controller); - UTEST_CHECK(test, p_controller.get() != 0); - UTEST_CHECK(test, !p_controller->tasks().empty()); - - // get the list of SE3 tasks (to test the tracking) - auto se3_tasks = parse_tasks(c_config); - UTEST_CHECK(test, !se3_tasks.empty()); - - // get the behavior (trajectories) - y::Node b_config = IWBC_CHECK(y::LoadFile(behavior_path)); - auto behavior_name = IWBC_CHECK(b_config["BEHAVIOR"]["name"].as()); - auto behavior = inria_wbc::behaviors::Factory::instance().create(behavior_name, controller, b_config); - UTEST_CHECK(test, behavior); - - // a few useful variables - auto all_dofs = controller->all_dofs(); - auto floating_base = all_dofs; - floating_base.resize(6); - auto controllable_dofs = controller->controllable_dofs(); - uint ncontrollable = controllable_dofs.size(); - double time_simu = 0, time_cmd = 0, time_solver = 0; - int it_simu = 0, it_cmd = 0; - - // init the robot - robot->set_positions(controller->q0(), all_dofs); - - // ----------------------- the main loop ----------------------- - using namespace std::chrono; - inria_wbc::controllers::SensorData sensor_data; - - Eigen::VectorXd cmd; - while (simu.scheduler().next_time() < cst::duration) { - double time_step_solver = 0, time_step_cmd = 0, time_step_simu = 0; - - // update the sensors - inria_wbc::controllers::SensorData sensor_data; - // left foot - sensor_data["lf_torque"] = Eigen::Vector3d::Zero(); - sensor_data["lf_force"] = Eigen::Vector3d::Zero(); - // right foot - sensor_data["rf_torque"] = Eigen::Vector3d::Zero(); - sensor_data["rf_force"] = Eigen::Vector3d::Zero(); - // accelerometer - sensor_data["acceleration"] = Eigen::Vector3d::Zero(); - sensor_data["velocity"] = Eigen::Vector3d::Zero(); - // joint positions (excluding floating base) - sensor_data["positions"] = robot->skeleton()->getPositions().tail(ncontrollable); - - // command - if (simu.schedule(simu.control_freq())) { - auto t1_solver = high_resolution_clock::now(); - behavior->update(sensor_data); - auto q = controller->q(false); - auto t2_solver = high_resolution_clock::now(); - time_step_solver = duration_cast(t2_solver - t1_solver).count(); - - auto t1_cmd = high_resolution_clock::now(); - if (actuator_type == "velocity" || actuator_type == "servo") - cmd = inria_wbc::robot_dart::compute_velocities(robot, q, cst::dt, controller->all_dofs(false)); - else if (actuator_type == "spd") - cmd = inria_wbc::robot_dart::compute_spd(robot, q, cst::dt, controller->all_dofs(false)); - else // torque - cmd = controller->tau(false); - - auto t2_cmd = high_resolution_clock::now(); - time_step_cmd = duration_cast(t2_cmd - t1_cmd).count(); - - robot->set_commands(controller->filter_cmd(cmd).tail(ncontrollable), controllable_dofs); - ++it_cmd; - } - - // step the simulation - { - auto t1_simu = high_resolution_clock::now(); - simu.step_world(); - auto t2_simu = high_resolution_clock::now(); - time_step_simu = duration_cast(t2_simu - t1_simu).count(); - ++it_simu; - } - - // tracking information - for (auto& t : se3_tasks) { - Eigen::VectorXd pos_dart, pos_tsid; - if (robot->joint(t.tracked)) { - auto node = robot->joint(t.tracked)->getParentBodyNode(); - auto tf_bd_world = node->getWorldTransform(); - auto tf_bd_joint = robot->joint(t.tracked)->getTransformFromParentBodyNode(); - pos_dart = (tf_bd_world * tf_bd_joint).translation(); - pos_tsid = controller->model_joint_pos(t.tracked).translation(); - } - else { - pos_dart = robot->body_pose(t.tracked).translation(); - // Q: doesn't TSID create a frame for each joint? - pos_tsid = controller->model_frame_pos(t.tracked).translation(); - } - // masks? // TODO also compute angular error - auto pos_ref = p_controller->get_se3_ref(t.name).translation(); - if (t.weight > 0) { - double error_dart = 0.; - double error_tsid = 0.; - IWBC_ASSERT(t.mask.size() == 6, "mask size=", t.mask.size()); - for (int i = 0; i < 3; ++i) { - if (t.mask[i] == '1') { - error_dart += (pos_dart[i] - pos_ref[i]) * (pos_dart[i] - pos_ref[i]); - error_tsid += (pos_tsid[i] - pos_ref[i]) * (pos_tsid[i] - pos_ref[i]); - } - } - t.error_dart += sqrt(error_dart); - t.error_tsid += sqrt(error_tsid); - } - } - - // timing information - time_simu += time_step_simu; - time_cmd += time_step_cmd; - time_solver += time_step_solver; - } - // time report - double t_sim = time_simu / it_simu / 1000.; - double t_cmd = time_cmd / it_cmd / 1000.; - double t_solver = time_solver / it_cmd / 1000.; - - UTEST_INFO(test, std::string("TIME: ") + "\ttotal:" + std::to_string((time_simu + time_cmd + time_solver) / 1e6) + " s" + "\t[" + std::to_string((t_sim + t_cmd + t_solver)) + " ms / it.]" + "\tsimu: " + std::to_string(t_sim) + " ms/it" + "\tsolver:" + std::to_string(t_solver) + " ms/it" + "\tcmd:" + std::to_string(t_cmd) + " ms/it"); - if (yout) - (*yout) << y::Key << "time" << y::Value - << std::vector{(time_simu + time_cmd + time_solver) / 1e6, (t_sim + t_cmd + t_solver), t_sim, t_solver, t_cmd}; - - // error report for se3 tasks - for (auto& t : se3_tasks) { - t.error_tsid /= simu.scheduler().current_time() * 1000; - t.error_dart /= simu.scheduler().current_time() * 1000; - UTEST_INFO(test, std::string("task: ") + t.name + " [" + t.tracked + "]" + "pos error TSID:" + std::to_string(t.error_tsid) + " " + "pos error DART:" + std::to_string(t.error_dart) + " " + "\t(mask =" + t.mask + ",weight=" + std::to_string(t.weight) + ")"); - if (yout) - (*yout) << y::Key << t.name << y::Value << std::vector{t.error_tsid, t.error_dart}; - } - - // comparisons to the reference values - try { - // SE3 tasks - for (auto& t : se3_tasks) { - auto e = IWBC_CHECK(ref[t.name].as>()); - UTEST_CHECK_MESSAGE(test, t.name + " tolerance --> " + std::to_string(t.error_tsid - e[0]) + " > " + std::to_string(cst::tolerance), t.error_tsid <= e[0] + cst::tolerance); - UTEST_CHECK_MESSAGE(test, t.name + " tolerance --> " + std::to_string(t.error_dart - e[1]) + " > " + std::to_string(cst::tolerance), t.error_dart <= e[1] + cst::tolerance); - } - } - catch (std::exception& e) { - UTEST_ERROR(test, std::string("error in ref comparison:") + e.what()); - } -} - -void test_behavior(utest::test_t test, - std::string controller_path, - std::string behavior_path, - std::string actuators, - std::string coll, - std::string urdf, - y::Node ref, - std::shared_ptr yout) -{ - if (yout) - (*yout) << y::Key << urdf << y::BeginMap; - - y::Node node; - try { - node = ref[controller_path][behavior_path][actuators][coll][urdf]; - } - catch (std::exception& e) { - UTEST_ERROR(test, std::string("error in getting the ref node:") + e.what()); - node = ref; - } - // create the simulator and the robot - std::vector> packages = {{"franka_description", "franka/franka_description"}}; - auto robot = std::make_shared(urdf, packages); - if (actuators == "spd") - robot->set_actuator_types("torque"); - else - robot->set_actuator_types(actuators); - robot->fix_to_world(); - robot->set_position_enforced(true); - - robot_dart::RobotDARTSimu simu(cst::dt); - simu.set_collision_detector(coll); - simu.set_control_freq(cst::frequency); - simu.add_robot(robot); - simu.add_checkerboard_floor(); - -#ifdef GRAPHIC - auto graphics = std::make_shared(); - simu.set_graphics(graphics); - graphics->look_at({3.5, -2, 2.2}, {0., 0., 1.4}); - // we always save the video (useful for bug reports) - graphics->record_video("test_result_franka.mp4"); -#endif - - try { - test_behavior(test, controller_path, behavior_path, simu, robot, actuators, node, yout); - } - catch (std::exception& e) { - UTEST_ERROR(test, std::string("exception when running the behavior:") + e.what()); - } - if (yout) - (*yout) << y::EndMap; -} - -int main(int argc, char** argv) -{ - - // program options - namespace po = boost::program_options; - po::options_description desc("Test_controller options"); - // clang-format off - desc.add_options() - ("generate_ref,g", "generate a reference file") - ("n_threads,n", po::value()->default_value(-1), "run tests in parallel (default = number of cores)") - ("single,s", po::value >()->multitoken(), "run a single test, args: controller_path behavior_path actuators coll enable_stabilizer urdf") - ; - // clang-format on - po::variables_map vm; - try { - po::store(po::parse_command_line(argc, argv, desc), vm); - po::notify(vm); - } - catch (po::too_many_positional_options_error& e) { - // A positional argument like `opt2=option_value_2` was given - std::cerr << e.what() << std::endl; - std::cerr << desc << std::endl; - return 1; - } - catch (po::error_with_option_name& e) { - // Another usage error occurred - std::cerr << e.what() << std::endl; - std::cerr << desc << std::endl; - return 1; - } - - int n_threads = vm["n_threads"].as(); -#ifdef GRAPHIC - n_threads = 1; -#endif - - utest::TestSuite test_suite; - - // we load the reference - y::Node ref; - - try { - ref = IWBC_CHECK(y::LoadFile(cst::ref_path)); - } - catch (std::exception& e) { - std::cerr << "cannot load reference file, path=" << cst::ref_path << " what:" << e.what() << std::endl; - } - // we write the results in yaml file, for future comparisons and analysis - auto yout = std::make_shared(); - (*yout) << y::BeginMap; - // the YAMl file has a timestamp (so that we can archive them) - (*yout) << y::Key << "timestamp" << y::Value << date(); - - if (vm.count("single")) { - auto args = vm["single"].as>(); - if (args.size() != 5) { - std::cerr << " Wrong number of arguments: (" << args.size() << ")" - << "controller_path behavior_path actuators coll urdf" - << std::endl; - return 1; - } - std::string name = args[0] + " " - + args[1] + " " - + args[2] + " " - + args[3] + " " - + args[4] + " "; - std::cout << "Single test:" << name << std::endl; - auto test1 = utest::make_test(name); -#ifdef GRAPHIC - test_behavior(test1, args[0], args[1], args[2], args[3], args[4], ref, std::shared_ptr()); -#else - UTEST_REGISTER(test_suite, test1, test_behavior(test1, args[0], args[1], args[2], args[3], args[4], ref, std::shared_ptr())); - test_suite.run(1, true); - utest::write_report(test_suite, std::cout, true); -#endif - return 0; - } - - ///// the default behavior is to run all the combinations in different threads - - // this is relative to the "tests" directory - std::string controller = "../../etc/franka/pos_tracker.yaml"; - auto behaviors = {"../../etc/franka/cartesian_line.yaml"}; - auto collision = {"fcl"}; - auto actuators = {"servo", "velocity", "spd"}; - std::string urdf = "franka/franka.urdf"; - - (*yout) << y::Key << controller << y::Value << y::BeginMap; - for (auto& b : behaviors) { - (*yout) << y::Key << b << y::Value << y::BeginMap; - for (auto& a : actuators) { - (*yout) << y::Key << a << y::Value << y::BeginMap; - for (auto& c : collision) { - (*yout) << y::Key << c << y::Value << y::BeginMap; - std::string name = controller + " " - + b + " " - + a + " " - + c; - auto test1 = utest::make_test(name + " " + urdf); - - if (vm.count("generate_ref")) { - std::cout << "generating..." << std::endl; - test_behavior(test1, controller, b, a, c, urdf, ref, yout); - } - else { - UTEST_REGISTER(test_suite, test1, test_behavior(test1, controller, b, a, c, urdf, ref, std::shared_ptr())); - } - (*yout) << y::EndMap; - } - (*yout) << y::EndMap; - } - } - (*yout) << y::EndMap; - (*yout) << y::EndMap; - - auto fname = std::string("test_results-franka-") + date() + ".yaml"; - std::ofstream ofs(fname.c_str()); - ofs << yout->c_str(); - - if (!vm.count("generate_ref")) { - test_suite.run(n_threads); - utest::write_report(test_suite, std::cout, true); - std::cout << "------------ SUMMARY ------------" << std::endl; - utest::write_report(test_suite, std::cout, false); - if(test_suite.success()) - return 0; - else - return 1; - } - return 0; -} diff --git a/tests/test_talos.cpp b/tests/test_talos.cpp deleted file mode 100644 index fd7e4947..00000000 --- a/tests/test_talos.cpp +++ /dev/null @@ -1,593 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#ifdef GRAPHIC -#include -#endif - -#include "inria_wbc/behaviors/behavior.hpp" -#include "inria_wbc/robot_dart/cmd.hpp" - -#include "inria_wbc/behaviors/behavior.hpp" -#include "inria_wbc/controllers/pos_tracker.hpp" -#include "inria_wbc/controllers/talos_pos_tracker.hpp" -#include "inria_wbc/exceptions.hpp" -#include "inria_wbc/robot_dart/cmd.hpp" -#include "inria_wbc/robot_dart/external_collision_detector.hpp" -#include "inria_wbc/robot_dart/self_collision_detector.hpp" -#include "inria_wbc/robot_dart/utils.hpp" - -#include "utest.hpp" - -namespace cst { - static constexpr double dt = 0.001; - static constexpr double duration = 10; - static constexpr double frequency = 1000; - static constexpr double tolerance = 5e-3; - static const std::string ref_path = "../../tests/ref_test_talos.yaml"; - static const std::string base_path = "../../etc/talos/"; - -} // namespace cst - -namespace inria_wbc { - namespace tests { - struct SE3TaskData { - std::string name; - std::string tracked; - std::string mask; - double weight; - double error_dart = 0.0; - double error_tsid = 0.0; - }; - } // namespace tests -} // namespace inria_wbc - -namespace y = YAML; - -// for timestamps -inline std::string date() -{ - char date[30]; - time_t date_time; - time(&date_time); - strftime(date, 30, "%Y-%m-%d_%H-%M-%S", localtime(&date_time)); - return std::string(date); -} - -// we parse again: -// - to check that the controller parses as expected -// - and to get additional info (mask, etc.) -std::vector parse_tasks(const y::Node& config) -{ - std::vector tasks; - auto c = IWBC_CHECK(config["CONTROLLER"]); - auto path = IWBC_CHECK(c["base_path"].as()); - auto task_file = IWBC_CHECK(c["tasks"].as()); - auto p = path + "/" + task_file; - auto task_list = IWBC_CHECK(y::LoadFile(p)); - for (auto it = task_list.begin(); it != task_list.end(); ++it) { - auto name = IWBC_CHECK(it->first.as()); - auto type = IWBC_CHECK(it->second["type"].as()); - if (type == "se3") { - auto weight = IWBC_CHECK(it->second["weight"].as()); - auto mask = IWBC_CHECK(it->second["mask"].as()); - auto tracked = IWBC_CHECK(it->second["tracked"].as()); - tasks.push_back({name, tracked, mask, weight}); - } - } - - // std::cout << "SE3 TASKS (frames): "; - // for (auto& x : tasks) - // std::cout << x.name << "[" << x.tracked << "] "; - // std::cout << std::endl; - - return tasks; -} - -void test_behavior(utest::test_t test, - const std::string& controller_path, - const std::string& behavior_path, - robot_dart::RobotDARTSimu& simu, - const std::shared_ptr& robot, - const std::string& actuator_type, - const y::Node& ref, - bool enable_stabilizer, - std::shared_ptr yout, - bool verbose = false) -{ - // ----------------------- init ----------------------- - - y::Node c_config = IWBC_CHECK(y::LoadFile(controller_path)); - auto initial_value = IWBC_CHECK(c_config["CONTROLLER"]["stabilizer"]["activated"].as()); - c_config["CONTROLLER"]["stabilizer"]["activated"] = enable_stabilizer ? "true" : "false"; - c_config["CONTROLLER"]["base_path"] = cst::base_path; - c_config["CONTROLLER"]["urdf"] = robot->model_filename(); - c_config["CONTROLLER"]["mimic_dof_names"] = robot->mimic_dof_names(); - c_config["CONTROLLER"]["verbose"] = verbose; - - // get the controller - auto controller_name = IWBC_CHECK(c_config["CONTROLLER"]["name"].as()); - if (actuator_type == "torque") // force closed-loop - c_config["CONTROLLER"]["closed_loop"] = true; - auto controller = inria_wbc::controllers::Factory::instance().create(controller_name, c_config); - //std::cout<<"test:"<< test <<" controller:"<(controller); - UTEST_CHECK(test, p_controller.get() != 0); - UTEST_CHECK(test, !p_controller->tasks().empty()); - - // get the list of SE3 tasks (to test the tracking) - auto se3_tasks = parse_tasks(c_config); - UTEST_CHECK(test, !se3_tasks.empty()); - // get the behavior (trajectories) - y::Node b_config = IWBC_CHECK(y::LoadFile(behavior_path)); - auto behavior_name = IWBC_CHECK(b_config["BEHAVIOR"]["name"].as()); - auto behavior = inria_wbc::behaviors::Factory::instance().create(behavior_name, controller, b_config); - UTEST_CHECK(test, behavior); - - // add sensors to the robot (robot_dart) - // Force/torque (feet) - auto ft_sensor_left = simu.add_sensor(robot, "leg_left_6_joint", cst::frequency); - auto ft_sensor_right = simu.add_sensor(robot, "leg_right_6_joint", cst::frequency); - // IMU - robot_dart::sensor::IMUConfig imu_config; - imu_config.body = robot->body_node("imu_link"); - imu_config.frequency = cst::frequency; - auto imu = simu.add_sensor(imu_config); - // Torque (joints) - std::vector> torque_sensors; - auto talos_tracker_controller = std::static_pointer_cast(controller); - for (const auto& joint : talos_tracker_controller->torque_sensor_joints()) - torque_sensors.push_back(simu.add_sensor(robot, joint, cst::frequency)); - - // a few useful variables - auto all_dofs = controller->all_dofs(); - auto floating_base = all_dofs; - floating_base.resize(6); - auto controllable_dofs = controller->controllable_dofs(); - uint ncontrollable = controllable_dofs.size(); - double time_simu = 0, time_cmd = 0, time_solver = 0; - int it_simu = 0, it_cmd = 0; - - // init the robot - robot->set_positions(controller->q0(), all_dofs); - - // create the collision detector to check that the robot do not collides - inria_wbc::robot_dart::SelfCollisionDetector collision_detector(robot); - std::map collision_map; - - std::map filter_body_names_pairs; - filter_body_names_pairs["leg_right_6_link"] = "BodyNode"; - filter_body_names_pairs["leg_left_6_link"] = "BodyNode"; - auto robot_floor = simu.robot(1); - inria_wbc::robot_dart::ExternalCollisionDetector floor_collision_detector(robot, robot_floor, filter_body_names_pairs); - std::map floor_collision_map; - - // ----------------------- the main loop ----------------------- - using namespace std::chrono; - inria_wbc::controllers::SensorData sensor_data; - Eigen::VectorXd tq_sensors = Eigen::VectorXd::Zero(torque_sensors.size()); - - double error_com_tsid = 0.0; - double error_com_dart = 0.0; - - Eigen::VectorXd cmd; - while (simu.scheduler().next_time() < cst::duration) { - double time_step_solver = 0, time_step_cmd = 0, time_step_simu = 0; - - // update the sensors from the simulator - for (auto tq_sens = torque_sensors.cbegin(); tq_sens < torque_sensors.cend(); ++tq_sens) - tq_sensors(std::distance(torque_sensors.cbegin(), tq_sens)) = (*tq_sens)->torques()(0, 0); - sensor_data["joints_torque"] = tq_sensors; - sensor_data["lf_torque"] = ft_sensor_left->torque(); - sensor_data["lf_force"] = ft_sensor_left->force(); - sensor_data["rf_torque"] = ft_sensor_right->torque(); - sensor_data["rf_force"] = ft_sensor_right->force(); - sensor_data["velocity"] = robot->com_velocity().tail<3>(); - sensor_data["positions"] = robot->positions(controller->controllable_dofs(false)); - sensor_data["joint_velocities"] = robot->velocities(controller->controllable_dofs(false)); - sensor_data["floating_base_position"] = inria_wbc::robot_dart::floating_base_pos(robot->positions()); - sensor_data["floating_base_velocity"] = inria_wbc::robot_dart::floating_base_vel(robot->velocities()); - sensor_data["imu_pos"] = imu->angular_position_vec(); - sensor_data["imu_vel"] = imu->angular_velocity(); - sensor_data["imu_acc"] = imu->linear_acceleration(); - - // command - if (simu.schedule(simu.control_freq())) { - auto t1_solver = high_resolution_clock::now(); - behavior->update(sensor_data); - auto q = controller->q(false); - auto t2_solver = high_resolution_clock::now(); - time_step_solver = duration_cast(t2_solver - t1_solver).count(); - - auto t1_cmd = high_resolution_clock::now(); - if (actuator_type == "velocity" || actuator_type == "servo") - cmd = inria_wbc::robot_dart::compute_velocities(robot, q, cst::dt, controller->all_dofs(false)); - else if (actuator_type == "spd") - cmd = inria_wbc::robot_dart::compute_spd(robot, q, cst::dt, controller->all_dofs(false)); - else // torque - cmd = controller->tau(false); - - auto t2_cmd = high_resolution_clock::now(); - time_step_cmd = duration_cast(t2_cmd - t1_cmd).count(); - - robot->set_commands(controller->filter_cmd(cmd).tail(ncontrollable), controllable_dofs); - ++it_cmd; - } - - // push the robot - std::vector pv = {1.0, 5.0}; - if (enable_stabilizer) { - for (uint i = 0; i < pv.size(); i++) { - if (simu.scheduler().current_time() > pv[i] && simu.scheduler().current_time() < pv[i] + 0.5) { - if (behavior_name == "walk-on-spot") { - if (i == 0) - robot->set_external_force("base_link", Eigen::Vector3d(-120, 0, 0)); - if (i == 1) - robot->set_external_force("base_link", Eigen::Vector3d(0, 180, 0)); - } - else { - if (i == 0) - robot->set_external_force("base_link", Eigen::Vector3d(-135, 0, 0)); - if (i == 1) - robot->set_external_force("base_link", Eigen::Vector3d(0, 200, 0)); - } - } - if (simu.scheduler().current_time() > pv[i] + 0.25) - robot->clear_external_forces(); - } - } - - // step the simulation - { - auto t1_simu = high_resolution_clock::now(); - simu.step_world(); - auto t2_simu = high_resolution_clock::now(); - time_step_simu = duration_cast(t2_simu - t1_simu).count(); - ++it_simu; - } - - // check that we do not generate any self-collision or floor collision - // this will do nothing with the fast URDF (no collision checks) - if (simu.collision_detector() != "dart") { - auto collision_list = collision_detector.collide(); - for (auto& s : collision_list) - collision_map[s] = true; - } - - auto head_z_diff = std::abs(controller->model_frame_pos("head_1_link").translation()(2) - robot->body_pose("head_1_link").translation()(2)); - std::vector floor_collision_list; - - if (head_z_diff > 0.75) - floor_collision_list.push_back("head_1_link"); - - if (simu.collision_detector() != "dart") { - auto list2 = floor_collision_detector.collide(); - floor_collision_list.insert(floor_collision_list.end(), list2.begin(), list2.end()); - } - - for (auto& s : floor_collision_list) - floor_collision_map[s] = true; - - // compute the CoM error - error_com_dart += (robot->com() - p_controller->com_task()->getReference().getValue()).norm(); - error_com_tsid += (p_controller->com() - p_controller->com_task()->getReference().getValue()).norm(); - - // tracking information - for (auto& t : se3_tasks) { - Eigen::VectorXd pos_dart, pos_tsid; - if (robot->joint(t.tracked)) { - auto node = robot->joint(t.tracked)->getParentBodyNode(); - auto tf_bd_world = node->getWorldTransform(); - auto tf_bd_joint = robot->joint(t.tracked)->getTransformFromParentBodyNode(); - pos_dart = (tf_bd_world * tf_bd_joint).translation(); - pos_tsid = controller->model_joint_pos(t.tracked).translation(); - } - else { - pos_dart = robot->body_pose(t.tracked).translation(); - // Q: doesn't TSID create a frame for each joint? - pos_tsid = controller->model_frame_pos(t.tracked).translation(); - } - // masks? // TODO also compute angular error - auto pos_ref = p_controller->get_se3_ref(t.name).translation(); - if (t.weight > 0) { - double error_dart = 0.; - double error_tsid = 0.; - IWBC_ASSERT(t.mask.size() == 6, "mask size=", t.mask.size()); - for (int i = 0; i < 3; ++i) { - if (t.mask[i] == '1') { - error_dart += (pos_dart[i] - pos_ref[i]) * (pos_dart[i] - pos_ref[i]); - error_tsid += (pos_tsid[i] - pos_ref[i]) * (pos_tsid[i] - pos_ref[i]); - } - } - t.error_dart += sqrt(error_dart); - t.error_tsid += sqrt(error_tsid); - } - } - - // timing information - time_simu += time_step_simu; - time_cmd += time_step_cmd; - time_solver += time_step_solver; - } - // time report - double t_sim = time_simu / it_simu / 1000.; - double t_cmd = time_cmd / it_cmd / 1000.; - double t_solver = time_solver / it_cmd / 1000.; - - UTEST_INFO(test, std::string("TIME: ") + "\ttotal:" + std::to_string((time_simu + time_cmd + time_solver) / 1e6) + " s" + "\t[" + std::to_string((t_sim + t_cmd + t_solver)) + " ms / it.]" + "\tsimu: " + std::to_string(t_sim) + " ms/it" + "\tsolver:" + std::to_string(t_solver) + " ms/it" + "\tcmd:" + std::to_string(t_cmd) + " ms/it"); - if (yout) - (*yout) << y::Key << "time" << y::Value - << std::vector{(time_simu + time_cmd + time_solver) / 1e6, (t_sim + t_cmd + t_solver), t_sim, t_solver, t_cmd}; - - // collision report - for (auto& x : collision_map) { - UTEST_ERROR(test, std::string("collision detected: ") + x.first); - } - - // floor collision report - for (auto& x : floor_collision_map) { - UTEST_ERROR(test, std::string("floor collision detected: ") + x.first); - } - - // error report for COM - error_com_tsid /= simu.scheduler().current_time() * 1000; - error_com_dart /= simu.scheduler().current_time() * 1000; - std::vector com_errors = {error_com_tsid, error_com_dart}; - //std::cout << "CoM: [tsid]" << error_com_tsid << " [dart]" << error_com_dart << std::endl; - if (yout) - (*yout) << y::Key << "com" << y::Value << std::vector{error_com_tsid, error_com_dart}; - - // error report for se3 tasks - for (auto& t : se3_tasks) { - t.error_tsid /= simu.scheduler().current_time() * 1000; - t.error_dart /= simu.scheduler().current_time() * 1000; - UTEST_INFO(test, std::string("task: ") + t.name + " [" + t.tracked + "]" + "pos error TSID:" + std::to_string(t.error_tsid) + " " + "pos error DART:" + std::to_string(t.error_dart) + " " + "\t(mask =" + t.mask + ",weight=" + std::to_string(t.weight) + ")"); - if (yout) - (*yout) << y::Key << t.name << y::Value << std::vector{t.error_tsid, t.error_dart}; - } - - // comparisons to the reference values - try { - // CoM - auto com = IWBC_CHECK(ref["com"].as>()); - if (enable_stabilizer) { - UTEST_WARN_MESSAGE(test, "CoM tolerance --> " + std::to_string(error_com_tsid - com[0]) + " > " + std::to_string(cst::tolerance), error_com_tsid <= com[0] + cst::tolerance); - UTEST_WARN_MESSAGE(test, "CoM tolerance--> " + std::to_string(error_com_dart - com[1]) + " > " + std::to_string(cst::tolerance), error_com_dart <= com[1] + cst::tolerance); - } - else { - UTEST_CHECK_MESSAGE(test, "CoM tolerance --> " + std::to_string(error_com_tsid - com[0]) + " > " + std::to_string(cst::tolerance), error_com_tsid <= com[0] + cst::tolerance); - UTEST_CHECK_MESSAGE(test, "CoM tolerance--> " + std::to_string(error_com_dart - com[1]) + " > " + std::to_string(cst::tolerance), error_com_dart <= com[1] + cst::tolerance); - } - // SE3 tasks - for (auto& t : se3_tasks) { - auto e = IWBC_CHECK(ref[t.name].as>()); - if (enable_stabilizer) { - UTEST_WARN_MESSAGE(test, t.name + " tolerance --> " + std::to_string(t.error_tsid - e[0]) + " > " + std::to_string(cst::tolerance), t.error_tsid <= e[0] + cst::tolerance); - UTEST_WARN_MESSAGE(test, t.name + " tolerance --> " + std::to_string(t.error_dart - e[1]) + " > " + std::to_string(cst::tolerance), t.error_dart <= e[1] + cst::tolerance); - } - else { - UTEST_CHECK_MESSAGE(test, t.name + " tolerance --> " + std::to_string(t.error_tsid - e[0]) + " > " + std::to_string(cst::tolerance), t.error_tsid <= e[0] + cst::tolerance); - UTEST_CHECK_MESSAGE(test, t.name + " tolerance --> " + std::to_string(t.error_dart - e[1]) + " > " + std::to_string(cst::tolerance), t.error_dart <= e[1] + cst::tolerance); - } - } - } - catch (std::exception& e) { - UTEST_ERROR(test, std::string("error in ref comparison:") + e.what()); - } -} - -void test_behavior(utest::test_t test, - std::string controller_path, - std::string behavior_path, - std::string actuators, - std::string coll, - std::string enable_stabilizer, - std::string urdf, - y::Node ref, - std::shared_ptr yout) -{ - //std::cout<<"test_behavior::test:"<> packages = {{"talos_description", "talos/talos_description"}}; - auto robot = std::make_shared(urdf, packages); - if (actuators == "spd") - robot->set_actuator_types("torque"); - else - robot->set_actuator_types(actuators); - robot_dart::RobotDARTSimu simu(cst::dt); - simu.set_collision_detector(coll); - simu.set_control_freq(cst::frequency); - simu.add_robot(robot); - simu.add_checkerboard_floor(); - bool stabilizer = (enable_stabilizer == "true") ? true : false; - -#ifdef GRAPHIC - auto graphics = std::make_shared(); - simu.set_graphics(graphics); - graphics->look_at({3.5, -2, 2.2}, {0., 0., 1.4}); - // we always save the video (useful for bug reports) - graphics->record_video("test_result_talos.mp4"); -#endif - - try { - test_behavior(test, controller_path, behavior_path, simu, robot, actuators, node, stabilizer, yout); - } - catch (std::exception& e) { - UTEST_ERROR(test, std::string("exception when running the behavior:") + e.what()); - } - if (yout) - (*yout) << y::EndMap; -} - -int main(int argc, char** argv) -{ - - // program options - namespace po = boost::program_options; - po::options_description desc("Test_controller options"); - // clang-format off - desc.add_options() - ("generate_ref,g", "generate a reference file") - ("n_threads,n", po::value()->default_value(-1), "run tests in parallel (default = number of cores)") - ("single,s", po::value >()->multitoken(), "run a single test, args: controller_path behavior_path actuators coll enable_stabilizer urdf") - ; - // clang-format on - po::variables_map vm; - try { - po::store(po::parse_command_line(argc, argv, desc), vm); - po::notify(vm); - } - catch (po::too_many_positional_options_error& e) { - // A positional argument like `opt2=option_value_2` was given - std::cerr << e.what() << std::endl; - std::cerr << desc << std::endl; - return 1; - } - catch (po::error_with_option_name& e) { - // Another usage error occurred - std::cerr << e.what() << std::endl; - std::cerr << desc << std::endl; - return 1; - } - - int n_threads = vm["n_threads"].as(); -#ifdef GRAPHIC - n_threads = 1; -#endif - - utest::TestSuite test_suite; - - // we load the reference - y::Node ref; - - try { - ref = IWBC_CHECK(y::LoadFile(cst::ref_path)); - } - catch (std::exception& e) { - std::cerr << "cannot load reference file, path=" << cst::ref_path << " what:" << e.what() << std::endl; - } - // we write the results in yaml file, for future comparisons and analysis - auto yout = std::make_shared(); - (*yout) << y::BeginMap; - // the YAMl file has a timestamp (so that we can archive them) - (*yout) << y::Key << "timestamp" << y::Value << date(); - - if (vm.count("single")) { - auto args = vm["single"].as>(); - if (args.size() != 6) { - std::cerr << " Wrong number of arguments: (" << args.size() << ")" - << "controller_path behavior_path actuators coll enable_stabilizer urdf" - << std::endl; - return 1; - } - std::string name = args[0] + " " - + args[1] + " " - + args[2] + " " - + args[3] + " " - + args[4] + " " - + args[5]; - std::cout << "Single test:" << name << std::endl; - auto test1 = utest::make_test(name); -#ifdef GRAPHIC - test_behavior(test1, args[0], args[1], args[2], args[3], args[4], args[5], ref, std::shared_ptr()); -#else - UTEST_REGISTER(test_suite, test1, test_behavior(test1, args[0], args[1], args[2], args[3], args[4], args[5], ref, std::shared_ptr())); - test_suite.run(1, true); - utest::write_report(test_suite, std::cout, true); -#endif - return 0; - } - - ///// the default behavior is to run all the combinations in different threads - - // this is relative to the "tests" directory - std::string controller = "../../etc/talos/talos_pos_tracker.yaml"; - auto behaviors = {"../../etc/talos/arm.yaml", "../../etc/talos/squat.yaml", "../../etc/talos/clapping.yaml", "../../etc/talos/walk_on_spot.yaml"}; - auto collision = {"fcl", "dart"}; - auto actuators = {"servo", "torque", "velocity", "spd"}; - std::vector stabilized = {"true", "false"}; - std::vector urdfs = {"talos/talos.urdf", "talos/talos_fast.urdf"}; - std::vector frequency = {500, 1000}; - - (*yout) << y::Key << controller << y::Value << y::BeginMap; - for (auto& b : behaviors) { - (*yout) << y::Key << b << y::Value << y::BeginMap; - for (auto& a : actuators) { - (*yout) << y::Key << a << y::Value << y::BeginMap; - for (auto& c : collision) { - (*yout) << y::Key << c << y::Value << y::BeginMap; - for (auto& s : stabilized) { - if (!(s == std::string("true") && a == std::string("torque"))) { - (*yout) << y::Key << "stabilized_" + s << y::Value << y::BeginMap; - std::string name = controller + " " - + b + " " - + a + " " - + c + " " - + "stab: " + s; - auto test1 = utest::make_test(name + " " + urdfs[1]); - auto test2 = utest::make_test(name + " " + urdfs[0]); - - if (vm.count("generate_ref")) { - std::cout << "generating..." << std::endl; - test_behavior(test1, controller, b, a, c, s, urdfs[1], ref, yout); - if (c != std::string("dart")) - test_behavior(test2, controller, b, a, c, s, urdfs[0], ref, yout); - } - else { - UTEST_REGISTER(test_suite, test1, test_behavior(test1, controller, b, a, c, s, urdfs[1], ref, std::shared_ptr())); - if (c != std::string("dart")) - UTEST_REGISTER(test_suite, test2, test_behavior(test2, controller, b, a, c, s, urdfs[0], ref, std::shared_ptr())); - } - (*yout) << y::EndMap; - } - } - (*yout) << y::EndMap; - } - (*yout) << y::EndMap; - } - (*yout) << y::EndMap; - } - (*yout) << y::EndMap; - (*yout) << y::EndMap; - - auto fname = std::string("test_results-") + date() + ".yaml"; - std::ofstream ofs(fname.c_str()); - ofs << yout->c_str(); - - if (!vm.count("generate_ref")) { - test_suite.run(n_threads); - utest::write_report(test_suite, std::cout, true); - std::cout << "------------ SUMMARY ------------" << std::endl; - utest::write_report(test_suite, std::cout, false); - if(test_suite.success()) - return 0; - else - return 1; - } - return 0; -} \ No newline at end of file From bd22ae19d63b08283ae0550736e12fcaeb643cdf Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Mon, 30 May 2022 11:26:26 +0200 Subject: [PATCH 30/46] handle collision checks --- tests/CMakeLists.txt | 2 +- tests/test_all_robots.cpp | 22 ++++++++++++++++++---- tests/utest.hpp | 2 ++ 3 files changed, 21 insertions(+), 5 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 15c3693d..e4a35115 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -8,7 +8,7 @@ set(BINARY ${CMAKE_PROJECT_NAME}_test) # the tests require Dart to run if(${COMPILE_ROBOT_DART_EXAMPLE} STREQUAL "ON") find_package(RobotDART REQUIRED COMPONENTS Simu OPTIONAL_COMPONENTS Magnum) - set(TEST_SOURCES test_determinism.cpp test_talos.cpp test_franka.cpp test_robot_model.cpp test_all_robots.cpp) + set(TEST_SOURCES test_determinism.cpp test_robot_model.cpp test_all_robots.cpp) endif() foreach(TEST ${TEST_SOURCES}) diff --git a/tests/test_all_robots.cpp b/tests/test_all_robots.cpp index 899c74e5..e021b926 100644 --- a/tests/test_all_robots.cpp +++ b/tests/test_all_robots.cpp @@ -126,6 +126,7 @@ void test_behavior(utest::test_t test, const std::string& behavior_path, const std::shared_ptr& robot, const FData& fdata, + bool expect_collision, bool verbose = false) { try { @@ -196,12 +197,19 @@ void test_behavior(utest::test_t test, { simu.step_world(); } + // check collision + if (p_controller->is_model_colliding()) { + UTEST_CHECK_MESSAGE(test, "unexpected collision", expect_collision == true); + break; + } } + if (expect_collision) + UTEST_CHECK(test, p_controller->is_model_colliding()); auto t = timer["controller"]; UTEST_INFO(test, "solver + behavior:" + std::to_string(t.time / t.iterations / 1000) + "ms " + "[" + std::to_string(t.min_time / 1000) + "," + std::to_string(t.max_time / 1000) + "]"); } catch (std::exception& e) { - UTEST_ERROR(test, std::string("error in ref comparison:") + e.what()); + UTEST_ERROR(test, std::string("Exception:") + e.what()); } } @@ -215,6 +223,7 @@ int main(int argc, char** argv) desc.add_options() ("n_threads,n", po::value()->default_value(-1), "run tests in parallel (default = number of cores)") ("single,s", po::value >()->multitoken(), "run a single test, args: robot_name controller_path behavior_path actuators") + ("verbose,v", po::value()->default_value(false), "verbose mode") ; // clang-format on po::variables_map vm; @@ -236,6 +245,7 @@ int main(int argc, char** argv) } int n_threads = vm["n_threads"].as(); + bool verbose = vm["verbose"].as(); utest::TestSuite test_suite; // paths are relative to the "tests" directory (to use make check) @@ -248,10 +258,11 @@ int main(int argc, char** argv) std::string name = "franka"; std::string controller_path = "pos_tracker.yaml"; auto behaviors = {"cartesian_line.yaml"}; + bool expect_collision = false; for (auto& behavior_path : behaviors) { auto test1 = utest::make_test("[" + name + "] " + behavior_path); auto franka = std::make_shared(); - UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, franka, sensor_data_franka, false)); + UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, franka, sensor_data_franka, expect_collision, verbose)); } } /////////// Talos robot @@ -268,7 +279,9 @@ int main(int argc, char** argv) for (auto& behavior_path : behaviors) { auto test1 = utest::make_test("[" + name + "] " + behavior_path); auto talos = std::make_shared(); - UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, talos, sensor_data_talos, false)); + bool expect_collision = (behavior_path == std::string("clapping.yaml")) ? true : false; + + UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, talos, sensor_data_talos, expect_collision, verbose)); } } @@ -285,7 +298,8 @@ int main(int argc, char** argv) for (auto& behavior_path : behaviors) { auto test1 = utest::make_test("[" + name + "] " + behavior_path); auto icub = std::make_shared(); - UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, icub, sensor_data_icub, false)); + bool expect_collision = false; + UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, icub, sensor_data_icub, expect_collision, verbose)); } } ///////// RUN everything diff --git a/tests/utest.hpp b/tests/utest.hpp index 6691dfa8..5bd69075 100644 --- a/tests/utest.hpp +++ b/tests/utest.hpp @@ -166,6 +166,8 @@ namespace utest { #define UTEST_CHECK_MESSAGE(test, message, to_test) __UTEST_CHECK_MESSAGE(test, message, to_test, utest::res::error) #define UTEST_CHECK(test, to_test) UTEST_CHECK_MESSAGE(test, "", to_test) +#define UTEST_ERROR_MESSAGE(test, message) __UTEST_CHECK_MESSAGE(test, message, false, utest::res::error) + #define UTEST_WARN(test, to_test) __UTEST_CHECK_MESSAGE(test, "", to_test, utest::res::warning) #define UTEST_WARN_MESSAGE(test, msg, to_test) __UTEST_CHECK_MESSAGE(test, msg, to_test, utest::res::warning) From 9e7124a86690ecb9a38e10a6c889eb4b52475ab9 Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Wed, 1 Jun 2022 17:44:50 +0200 Subject: [PATCH 31/46] added rnea and decode ft --- .gitignore | 3 +- include/inria_wbc/controllers/pos_tracker.hpp | 4 +- include/inria_wbc/utils/robot_model.hpp | 14 +++ include/tsid/contacts/contact-6d-ext.hpp | 2 + src/controllers/pos_tracker.cpp | 74 ++++++-------- src/utils/robot_model.cpp | 99 +++++++++++++++++++ tests/test_robot_model.cpp | 23 +++++ 7 files changed, 171 insertions(+), 48 deletions(-) diff --git a/.gitignore b/.gitignore index 58c89ec5..9caba06c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ build CMakeCache.txt -CMakeFiles \ No newline at end of file +CMakeFiles +.vscode/ diff --git a/include/inria_wbc/controllers/pos_tracker.hpp b/include/inria_wbc/controllers/pos_tracker.hpp index 02efe786..f5ea59a5 100644 --- a/include/inria_wbc/controllers/pos_tracker.hpp +++ b/include/inria_wbc/controllers/pos_tracker.hpp @@ -7,7 +7,7 @@ namespace inria_wbc { namespace controllers { - + // generic position tracker, for both fixed-base and floatin-base robots class PosTracker : public Controller { public: @@ -58,7 +58,7 @@ namespace inria_wbc { void remove_contact(const std::string& contact_name); void add_contact(const std::string& contact_name); - Eigen::VectorXd force_torque_from_solution(const std::string& foot); + Eigen::VectorXd force_torque_from_solution(const std::string& contact_name, bool remove_foot_mass = false, const std::string& sole_frame = ""); // this only removes the task from the TSID list of tasks (the task is not destroyed) // therefore you can re-add it later by using its name diff --git a/include/inria_wbc/utils/robot_model.hpp b/include/inria_wbc/utils/robot_model.hpp index 4999392a..87fefe18 100644 --- a/include/inria_wbc/utils/robot_model.hpp +++ b/include/inria_wbc/utils/robot_model.hpp @@ -60,6 +60,20 @@ namespace utils { pinocchio::Data::Matrix6x jacobian(const std::string& joint_name, const pinocchio::ReferenceFrame& reference_frame = pinocchio::WORLD); pinocchio::Data::Tensor3x hessian(const std::string& joint_name, const pinocchio::ReferenceFrame& reference_frame = pinocchio::WORLD); + //Compute torques from sensor_data external forces. + //It is here done for the two foot contacts but it could be generalized to less or more contacts if needed + //You could directly use pinocchio rnea if there is no external forces or no foot mass to add + void compute_rnea_double_support(const std::unordered_map& sensor_data, + const Eigen::VectorXd& q, + const Eigen::VectorXd& v, + const Eigen::VectorXd& a, + Eigen::VectorXd& tau_model, + bool add_foot_mass = true, + const std::string& left_ft_frame = "leg_left_6_joint", + const std::string& right_ft_frame = "leg_right_6_joint", + const std::string& left_sole_frame = "left_sole_link", + const std::string& right_sole_frame = "right_sole_link"); + protected: Configuration _config; diff --git a/include/tsid/contacts/contact-6d-ext.hpp b/include/tsid/contacts/contact-6d-ext.hpp index ba0dc7e0..ac47c5ae 100644 --- a/include/tsid/contacts/contact-6d-ext.hpp +++ b/include/tsid/contacts/contact-6d-ext.hpp @@ -22,6 +22,8 @@ namespace tsid { Matrix3x getContactPoints() { return m_contactPoints; } + Vector3 getContactNormal() { return m_contactNormal; } + Vector6 getForceReference() { return m_fRef; } }; } // namespace contacts diff --git a/src/controllers/pos_tracker.cpp b/src/controllers/pos_tracker.cpp index 1cecfa28..c5d6ac1f 100644 --- a/src/controllers/pos_tracker.cpp +++ b/src/controllers/pos_tracker.cpp @@ -16,7 +16,7 @@ #include #ifdef TSID_QPMAD_FOUND - #include +#include #endif #include @@ -83,23 +83,20 @@ namespace inria_wbc { ////////////////////Create an HQP solver ///////////////////////////////////// using solver_t = std::shared_ptr; - if(solver_to_use_ == "qpmad") - { - #ifdef TSID_QPMAD_FOUND + if (solver_to_use_ == "qpmad") { +#ifdef TSID_QPMAD_FOUND solver_ = solver_t(solvers::SolverHQPFactory::createNewSolver(solvers::SOLVER_HQP_QPMAD, "solver-qpmad")); - #else +#else IWBC_ERROR("'qpmad' solver is not available in tsid or in the system."); - #endif +#endif } - else if(solver_to_use_ == "eiquadprog") - { + else if (solver_to_use_ == "eiquadprog") { solver_ = solver_t(solvers::SolverHQPFactory::createNewSolver(solvers::SOLVER_HQP_EIQUADPROG_FAST, "solver-eiquadprog")); } - else - { + else { IWBC_ERROR("solver in configuration file must be either 'eiquadprog' or 'qpmad'."); } - + solver_->resize(tsid_->nVar(), tsid_->nEq(), tsid_->nIn()); ////////////////////Compute Problem Data at init ///////////////////////////// @@ -257,45 +254,32 @@ namespace inria_wbc { activated_contacts_.push_back(contact_name); } //returns output = [fx,fy,fz,tau_x,tau_y,tau_z] from tsid solution - //it corresponds to the force of contact(contact_name)->Contact6d::setForceReference(force); - Eigen::VectorXd PosTracker::force_torque_from_solution(const std::string& foot) + //when we supress foot mass it corresponds to F/T sensor data + Eigen::VectorXd PosTracker::force_torque_from_solution(const std::string& contact_name, bool remove_foot_mass, const std::string& sole_frame) { - IWBC_ASSERT(foot == "left" || foot == "right", "foot must be left or right"); - - std::string ct_name, tau_x_name, tau_y_name; - if (foot == "left") { - ct_name = "contact_lfoot"; - tau_x_name = "leg_left_6_joint"; - tau_y_name = "leg_left_5_joint"; - } - if (foot == "right") { - ct_name = "contact_rfoot"; - tau_x_name = "leg_right_6_joint"; - tau_y_name = "leg_right_5_joint"; - } - Eigen::Matrix force_tsid; force_tsid.setZero(); - IWBC_ASSERT(activated_contacts_forces_.find(ct_name) != activated_contacts_forces_.end(), ct_name, "not in activated_contacts_forces_"); - auto contact_force = activated_contacts_forces_[ct_name]; - int n_contact_points = contact_force.size() / 3; - for (int i = 0; i < n_contact_points; i++) { - force_tsid(0) += -contact_force(0 + i * 3); - force_tsid(1) += -contact_force(1 + i * 3); - force_tsid(2) += -contact_force(2 + i * 3); + IWBC_ASSERT(activated_contacts_forces_.find(contact_name) != activated_contacts_forces_.end(), contact_name, " not in activated_contacts_forces_"); + auto contact_force = activated_contacts_forces_[contact_name]; + auto generatorMatrix = contact(contact_name)->Contact6d::getForceGeneratorMatrix(); + force_tsid = generatorMatrix * contact_force; + + if (remove_foot_mass) { + // we retrieve the tracked frame from the contact task + auto ankle_frame = robot_->model().frames[contact(contact_name)->getMotionTask().frame_id()].name; + IWBC_ASSERT(robot_->model().existFrame(ankle_frame), "Frame " + ankle_frame + " does not exist in model."); + IWBC_ASSERT(robot_->model().existFrame(sole_frame), "Frame " + sole_frame + " does not exist in model."); + + auto ankle_world = tsid_->data().oMi[robot_->model().getJointId(ankle_frame)]; + auto sole_world = tsid_->data().oMf[robot_->model().getFrameId(sole_frame)]; + + auto foot_mass = robot_->model().inertias[robot_->model().getJointId(ankle_frame)].mass(); + // auto contact_normal = contact(contact_name)->getContactNormal(); + force_tsid.head(3) += foot_mass * robot_->model().gravity.linear();; + Eigen::Vector3d tmp = force_tsid.head(3); + force_tsid.tail(3) -= (ankle_world.translation() - sole_world.translation()).cross(tmp); } - - auto tau_vec = tau(false); - auto dofs_names = all_dofs(false); - - auto it = std::find(dofs_names.begin(), dofs_names.end(), tau_x_name); - IWBC_ASSERT(it != dofs_names.end(), tau_x_name, "not in dofs_names"); - force_tsid(3) = -tau_vec[std::distance(dofs_names.begin(), it)]; - it = std::find(dofs_names.begin(), dofs_names.end(), tau_y_name); - IWBC_ASSERT(it != dofs_names.end(), tau_y_name, "not in dofs_names"); - force_tsid(4) = tau_vec[std::distance(dofs_names.begin(), it)]; - return force_tsid; } diff --git a/src/utils/robot_model.cpp b/src/utils/robot_model.cpp index 7ae4d50c..0af2c7bc 100644 --- a/src/utils/robot_model.cpp +++ b/src/utils/robot_model.cpp @@ -135,5 +135,104 @@ namespace utils { return pinocchio::getJointKinematicHessian(_model, _data, id, reference_frame); } + void RobotModel::compute_rnea_double_support(const std::unordered_map& sensor_data, + const Eigen::VectorXd& q, + const Eigen::VectorXd& v, + const Eigen::VectorXd& a, + Eigen::VectorXd& tau_model, + bool add_foot_mass, + const std::string& left_ft_frame, + const std::string& right_ft_frame, + const std::string& left_sole_frame, + const std::string& right_sole_frame) + { + pinocchio::computeJointJacobians(_model, _data, q); + pinocchio::updateFramePlacements(_model, _data); + + //compute torques with no external forces + pinocchio::rnea(_model, _data, q, v, a); + Eigen::VectorXd tau(_data.tau); + + //initiallize outputs + tau_model.resize(tau.size()); + tau_model.setZero(); + + //get F/T sensor data from both feet + Eigen::Vector3d right_force = Eigen::Vector3d::Zero(); + Eigen::Vector3d right_torque = Eigen::Vector3d::Zero(); + Eigen::Vector3d left_force = Eigen::Vector3d::Zero(); + Eigen::Vector3d left_torque = Eigen::Vector3d::Zero(); + + if((sensor_data.find("lf_force") == sensor_data.end()) || (sensor_data.find("rf_force") == sensor_data.end()) || + (sensor_data.find("lf_torque") == sensor_data.end()) || (sensor_data.find("rf_torque") == sensor_data.end())) + throw IWBC_EXCEPTION("when FT is missing in fext_map"); + + //Get data for the right foot + right_force = sensor_data.at("rf_force").col(0).head(3); + right_torque = sensor_data.at("rf_torque").col(0).head(3); + std::string right_frame = right_ft_frame; + + //Add the mass of the foot to sensor_data + if(add_foot_mass) + { + if (!_model.existFrame(right_ft_frame)) + throw IWBC_EXCEPTION("Frame name ", right_ft_frame, "is not in model"); + if (!_model.existFrame(right_sole_frame)) + throw IWBC_EXCEPTION("Frame name ", right_sole_frame, "is not in model"); + + auto ankle_world = _data.oMi[_model.getJointId(right_ft_frame)]; + auto sole_world = _data.oMf[_model.getFrameId(right_sole_frame)]; + + float mass_to_add = _model.inertias[_model.getJointId(right_ft_frame)].mass(); + //TODO hypothesis foot is on planar surface here ? + right_force -= mass_to_add * _model.gravity.linear(); + right_torque += (ankle_world.translation() - sole_world.translation()).cross(right_force); + right_frame = right_sole_frame; + } + + //Get data for the left foot + left_force = sensor_data.at("lf_force").col(0).head(3); + left_torque = sensor_data.at("lf_torque").col(0).head(3); + std::string left_frame = left_ft_frame; + + //Add the mass of the foot to sensor_data + if(add_foot_mass) + { + if (!_model.existFrame(left_ft_frame)) + throw IWBC_EXCEPTION("Frame name ", left_ft_frame, "is not in model"); + if (!_model.existFrame(left_sole_frame)) + throw IWBC_EXCEPTION("Frame name ", left_sole_frame, "is not in model"); + + auto ankle_world = _data.oMi[_model.getJointId(left_ft_frame)]; + auto sole_world = _data.oMf[_model.getFrameId(left_sole_frame)]; + + float mass_to_add = _model.inertias[_model.getJointId(left_ft_frame)].mass(); + //TODO hypothesis foot is on planar surface here ? + left_force -= mass_to_add * _model.gravity.linear(); + left_torque += (ankle_world.translation() - sole_world.translation()).cross(left_force); + left_frame = left_sole_frame; + } + + //Compute the torque from external forces J^transpose*force + pinocchio::Data::Matrix6x J_right(pinocchio::Data::Matrix6x(6, _model.nv)); + J_right.fill(0.); + if (_model.existFrame(right_frame)) + pinocchio::getFrameJacobian(_model, _data, _model.getFrameId(right_frame), pinocchio::LOCAL, J_right); + + pinocchio::Data::Matrix6x J_left(pinocchio::Data::Matrix6x(6, _model.nv)); + J_left.fill(0.); + if (_model.existFrame(left_frame)) + pinocchio::getFrameJacobian(_model, _data, _model.getFrameId(left_frame), pinocchio::LOCAL, J_left); + + pinocchio::Force f_right(right_force, right_torque); + pinocchio::Force f_left(left_force, left_torque); + tau -= J_right.transpose() * f_right.toVector(); + tau -= J_left.transpose() * f_left.toVector(); + + tau_model = tau; + + }; + + } // namespace utils } // namespace inria_wbc diff --git a/tests/test_robot_model.cpp b/tests/test_robot_model.cpp index 03918e4b..59042fba 100644 --- a/tests/test_robot_model.cpp +++ b/tests/test_robot_model.cpp @@ -56,6 +56,26 @@ void test_jacobians_update(utest::test_t test, inria_wbc::utils::RobotModel robo UTEST_CHECK_NO_EXCEPTION(test, robot_model.hessian("arm_right_1_joint")); } +void test_compute_rnea(utest::test_t test, inria_wbc::utils::RobotModel robot_model) +{ + Eigen::VectorXd q(robot_model.nq()), dq(robot_model.nv()), ddq(robot_model.nv()), tau_model; + q.setZero(); + dq.setZero(); + ddq.setZero(); + + robot_model.update(q, dq, ddq, true, true); + + std::unordered_map sensor_data; + Eigen::Vector3d data = Eigen::Vector3d::Zero(); + sensor_data["lf_torque"] = data; + sensor_data["rf_torque"] = data; + data(2) = 442.0; + sensor_data["lf_force"] = data; + sensor_data["rf_force"] = data; + + UTEST_CHECK_NO_EXCEPTION(test, robot_model.compute_rnea_double_support(sensor_data, q, dq ,ddq , tau_model)); +} + int main(int argc, char** argv) { @@ -101,6 +121,9 @@ int main(int argc, char** argv) auto jac_update_test = utest::make_test("robot_model kinematics update"); UTEST_REGISTER(test_suite, jac_update_test, test_jacobians_update(jac_update_test, robot_model)); + auto test_rnea = utest::make_test("test rnea computation"); + UTEST_REGISTER(test_suite, test_rnea, test_compute_rnea(test_rnea, robot_model)); + test_suite.run(); utest::write_report(test_suite, std::cout, true); From d455687b6378cdb3fac308e15acf5bdec942eeee Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Thu, 2 Jun 2022 09:57:58 +0200 Subject: [PATCH 32/46] compute_rnea_double_support returns Eigen::VectorXd --- include/inria_wbc/utils/robot_model.hpp | 13 ++++++------- src/utils/robot_model.cpp | 9 ++------- tests/test_robot_model.cpp | 4 ++-- 3 files changed, 10 insertions(+), 16 deletions(-) diff --git a/include/inria_wbc/utils/robot_model.hpp b/include/inria_wbc/utils/robot_model.hpp index 87fefe18..48456e97 100644 --- a/include/inria_wbc/utils/robot_model.hpp +++ b/include/inria_wbc/utils/robot_model.hpp @@ -63,16 +63,15 @@ namespace utils { //Compute torques from sensor_data external forces. //It is here done for the two foot contacts but it could be generalized to less or more contacts if needed //You could directly use pinocchio rnea if there is no external forces or no foot mass to add - void compute_rnea_double_support(const std::unordered_map& sensor_data, + Eigen::VectorXd compute_rnea_double_support(const std::unordered_map& sensor_data, const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Eigen::VectorXd& a, - Eigen::VectorXd& tau_model, - bool add_foot_mass = true, - const std::string& left_ft_frame = "leg_left_6_joint", - const std::string& right_ft_frame = "leg_right_6_joint", - const std::string& left_sole_frame = "left_sole_link", - const std::string& right_sole_frame = "right_sole_link"); + bool add_foot_mass, + const std::string& left_ft_frame, + const std::string& right_ft_frame, + const std::string& left_sole_frame, + const std::string& right_sole_frame); protected: diff --git a/src/utils/robot_model.cpp b/src/utils/robot_model.cpp index 0af2c7bc..bca8e4fe 100644 --- a/src/utils/robot_model.cpp +++ b/src/utils/robot_model.cpp @@ -135,11 +135,10 @@ namespace utils { return pinocchio::getJointKinematicHessian(_model, _data, id, reference_frame); } - void RobotModel::compute_rnea_double_support(const std::unordered_map& sensor_data, + Eigen::VectorXd RobotModel::compute_rnea_double_support(const std::unordered_map& sensor_data, const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Eigen::VectorXd& a, - Eigen::VectorXd& tau_model, bool add_foot_mass, const std::string& left_ft_frame, const std::string& right_ft_frame, @@ -153,10 +152,6 @@ namespace utils { pinocchio::rnea(_model, _data, q, v, a); Eigen::VectorXd tau(_data.tau); - //initiallize outputs - tau_model.resize(tau.size()); - tau_model.setZero(); - //get F/T sensor data from both feet Eigen::Vector3d right_force = Eigen::Vector3d::Zero(); Eigen::Vector3d right_torque = Eigen::Vector3d::Zero(); @@ -229,7 +224,7 @@ namespace utils { tau -= J_right.transpose() * f_right.toVector(); tau -= J_left.transpose() * f_left.toVector(); - tau_model = tau; + return tau; }; diff --git a/tests/test_robot_model.cpp b/tests/test_robot_model.cpp index 59042fba..fd56e57a 100644 --- a/tests/test_robot_model.cpp +++ b/tests/test_robot_model.cpp @@ -58,7 +58,7 @@ void test_jacobians_update(utest::test_t test, inria_wbc::utils::RobotModel robo void test_compute_rnea(utest::test_t test, inria_wbc::utils::RobotModel robot_model) { - Eigen::VectorXd q(robot_model.nq()), dq(robot_model.nv()), ddq(robot_model.nv()), tau_model; + Eigen::VectorXd q(robot_model.nq()), dq(robot_model.nv()), ddq(robot_model.nv()); q.setZero(); dq.setZero(); ddq.setZero(); @@ -73,7 +73,7 @@ void test_compute_rnea(utest::test_t test, inria_wbc::utils::RobotModel robot_mo sensor_data["lf_force"] = data; sensor_data["rf_force"] = data; - UTEST_CHECK_NO_EXCEPTION(test, robot_model.compute_rnea_double_support(sensor_data, q, dq ,ddq , tau_model)); + UTEST_CHECK_NO_EXCEPTION(test, robot_model.compute_rnea_double_support(sensor_data, q, dq ,ddq, "true","leg_left_6_joint", "leg_right_6_joint","left_sole_link","right_sole_link")); } From 104a1a0aa292caea035b3f523910826f81d34285 Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Thu, 2 Jun 2022 10:34:55 +0200 Subject: [PATCH 33/46] small diffs --- etc/tiago/cartesian_line.yaml | 4 ++-- src/controllers/pos_tracker.cpp | 22 ++++++++++------------ src/robot_dart/tiago.cpp | 2 +- 3 files changed, 13 insertions(+), 15 deletions(-) diff --git a/etc/tiago/cartesian_line.yaml b/etc/tiago/cartesian_line.yaml index b7129ae3..e97751fa 100644 --- a/etc/tiago/cartesian_line.yaml +++ b/etc/tiago/cartesian_line.yaml @@ -1,7 +1,7 @@ BEHAVIOR: name : generic::cartesian task_names: [ee] - trajectory_duration: 100.0 - relative_targets_pos: [[0., 0.5, 0.9]] + trajectory_duration: 5.0 + relative_targets_pos: [[0., 0.5, 0.5]] relative_targets_rpy: [[0.0, 0.0, 0.0]] loop: true \ No newline at end of file diff --git a/src/controllers/pos_tracker.cpp b/src/controllers/pos_tracker.cpp index 5cb8c951..700a739c 100644 --- a/src/controllers/pos_tracker.cpp +++ b/src/controllers/pos_tracker.cpp @@ -16,7 +16,7 @@ #include #ifdef TSID_QPMAD_FOUND - #include +#include #endif #include @@ -65,7 +65,8 @@ namespace inria_wbc { auto ref_map = robot_->model().referenceConfigurations; IWBC_ASSERT(ref_map.find(ref_config) != ref_map.end(), "The following reference config is not in ref_map : ", ref_config); q_tsid_ = ref_map[ref_config]; - std::cout<<"q_tsid ref:"<; - if(solver_to_use_ == "qpmad") - { - #ifdef TSID_QPMAD_FOUND + if (solver_to_use_ == "qpmad") { +#ifdef TSID_QPMAD_FOUND solver_ = solver_t(solvers::SolverHQPFactory::createNewSolver(solvers::SOLVER_HQP_QPMAD, "solver-qpmad")); - #else +#else IWBC_ERROR("'qpmad' solver is not available in tsid or in the system."); - #endif +#endif } - else if(solver_to_use_ == "eiquadprog") - { + else if (solver_to_use_ == "eiquadprog") { solver_ = solver_t(solvers::SolverHQPFactory::createNewSolver(solvers::SOLVER_HQP_EIQUADPROG_FAST, "solver-eiquadprog")); } - else - { + else { IWBC_ERROR("solver in configuration file must be either 'eiquadprog' or 'qpmad'."); } - + solver_->resize(tsid_->nVar(), tsid_->nEq(), tsid_->nIn()); ////////////////////Compute Problem Data at init ///////////////////////////// diff --git a/src/robot_dart/tiago.cpp b/src/robot_dart/tiago.cpp index cd3e01be..e503fbdb 100644 --- a/src/robot_dart/tiago.cpp +++ b/src/robot_dart/tiago.cpp @@ -33,7 +33,7 @@ int main(int argc, char* argv[]) po::options_description desc("Test_controller options"); // clang-format off desc.add_options() - ("actuators,a", po::value()->default_value("servo"), "actuator model torque/velocity/servo (always for position control) [default:servo]") + ("actuators,a", po::value()->default_value("spd"), "actuator model torque/velocity/servo (always for position control) [default:servo]") ("behavior,b", po::value()->default_value("../etc/tiago/cartesian_line.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/tiago/circular_cartesian.yam]") ("big_window,w", "use a big window (nicer but slower) [default:true]") ("check_self_collisions", "check the self collisions (print if a collision)") From 636209a0e1efbb61a2a32cf1dddcf4833d7d902c Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Thu, 2 Jun 2022 11:43:24 +0200 Subject: [PATCH 34/46] test ok on docker clang c++17 --- ...tracker.yaml => humanoid_pos_tracker.yaml} | 1 + etc/talos/randomize.yaml | 157 ++++++++++++++++++ etc/talos/traj_teleop1.yaml | 2 +- src/robot_dart/icub.cpp | 2 +- tests/CMakeLists.txt | 25 ++- tests/test_example_project.sh | 4 +- 6 files changed, 173 insertions(+), 18 deletions(-) rename etc/icub/{pos_tracker.yaml => humanoid_pos_tracker.yaml} (97%) create mode 100644 etc/talos/randomize.yaml mode change 100644 => 100755 tests/test_example_project.sh diff --git a/etc/icub/pos_tracker.yaml b/etc/icub/humanoid_pos_tracker.yaml similarity index 97% rename from etc/icub/pos_tracker.yaml rename to etc/icub/humanoid_pos_tracker.yaml index 14e1a2f5..a4e5fbd0 100644 --- a/etc/icub/pos_tracker.yaml +++ b/etc/icub/humanoid_pos_tracker.yaml @@ -1,5 +1,6 @@ CONTROLLER: name: humanoid-pos-tracker + solver: eiquadprog base_path: /home/pal/inria_wbc/etc/icub tasks: tasks.yaml urdf: icub.urdf diff --git a/etc/talos/randomize.yaml b/etc/talos/randomize.yaml new file mode 100644 index 00000000..ba5de749 --- /dev/null +++ b/etc/talos/randomize.yaml @@ -0,0 +1,157 @@ +head_yaw: + weight: 100.0 + type: se3 + tracked: head_2_joint + kp: 30.0 + mask: 000001 +head_pitch: + tracked: head_1_joint + weight: 100.0 + type: se3 + kp: 30.0 + mask: 000010 +head: + mask: 110000 + weight: 1.0 + kp: 1.0 + tracked: head_1_joint + type: se3 +rh: + weight: 10.0 + type: se3 + tracked: gripper_right_joint + kp: 30.0 + mask: 111111 +torso: + weight: 10.0 + type: se3 + tracked: torso_2_link + kp: 30.0 + mask: 000110 +lf: + weight: 1000.0 + type: se3 + tracked: leg_left_6_joint + kp: 30.0 + mask: 111111 +lh: + weight: 10.0 + tracked: gripper_left_joint + type: se3 + kp: 30.0 + mask: 111111 +com: + kp: 30.0 + type: com + weight: 1000.0 + mask: 111 +rf: + type: se3 + tracked: leg_right_6_joint + weight: 1000.0 + kp: 30.0 + mask: 111111 +posture: + type: posture + kp: 10.0 + weight: 1.75 + ref: inria_start +actuation_bounds: + type: actuation-bounds + weight: 10000 +momentum: + weight: 1000.0 + kp: 30.0 + type: momentum + mask: 000110 +bounds: + weight: 10000 + type: bounds +contact_lfoot: + lxp: 0.1 + kp: 30.0 + type: contact + joint: leg_left_6_joint + lyn: 0.069 + mu: 0.3 + lyp: 0.069 + lxn: 0.11 + lz: 0.107 + fmin: 5.0 + fmax: 1500.0 + normal: [0, 0, 1] +contact_rfoot: + normal: [0, 0, 1] + joint: leg_right_6_joint + kp: 30.0 + lz: 0.107 + lxn: 0.11 + type: contact + lxp: 0.1 + lyp: 0.069 + lyn: 0.069 + fmax: 1500.0 + fmin: 5.0 + mu: 0.3 +self_collision-left: + tracked: gripper_left_joint + avoided: + v_base_link_right: 0.115 + leg_right_4_joint: 0.1 + v_leg_left_3: 0.15 + v_leg_right_3: 0.15 + gripper_right_joint: 0.1 + arm_right_5_joint: 0.05 + base_link: 0.15 + torso_2_joint: 0.1 + leg_right_1_joint: 0.15 + leg_left_1_joint: 0.15 + leg_left_4_joint: 0.1 + v_base_link_left: 0.115 + type: self-collision + radius: 0.1 + weight: 500 + p: 3 + kp: 700.0 +self_collision-right: + type: self-collision + weight: 500 + tracked: gripper_right_joint + radius: 0.1 + avoided: + gripper_left_joint: 0.1 + arm_left_5_joint: 0.05 + v_leg_right_3: 0.15 + leg_right_1_joint: 0.15 + torso_2_joint: 0.1 + base_link: 0.15 + v_base_link_right: 0.115 + v_base_link_left: 0.115 + v_leg_left_3: 0.15 + leg_left_1_joint: 0.15 + leg_left_4_joint: 0.1 + leg_right_4_joint: 0.1 + p: 3 + kp: 700.0 +self_collision-arm-right: + type: self-collision + radius: 0.15 + avoided: + v_base_link_left: 0.115 + base_link: 0.15 + v_base_link_right: 0.115 + tracked: arm_right_4_joint + kp: 10.0 + p: 3 + weight: 10 +self_collision-arm-left: + avoided: + v_base_link_right: 0.115 + v_base_link_left: 0.115 + base_link: 0.15 + tracked: arm_left_4_joint + kp: 10.0 + radius: 0.15 + type: self-collision + weight: 10 + p: 3 \ No newline at end of file diff --git a/etc/talos/traj_teleop1.yaml b/etc/talos/traj_teleop1.yaml index dfec1eef..36e7794b 100644 --- a/etc/talos/traj_teleop1.yaml +++ b/etc/talos/traj_teleop1.yaml @@ -1,5 +1,5 @@ BEHAVIOR: name : generic::cartesian_traj - trajectories: traj_dance/traj_dance.yaml + trajectories: traj_teleop1/trajectory.yaml loop: true scale: 1 diff --git a/src/robot_dart/icub.cpp b/src/robot_dart/icub.cpp index 19ae2eff..7a4ee300 100644 --- a/src/robot_dart/icub.cpp +++ b/src/robot_dart/icub.cpp @@ -51,7 +51,7 @@ int main(int argc, char* argv[]) ("check_fall", "check if the robot has fallen (print if a collision)") ("collision,k", po::value()->default_value("fcl"), "collision engine [default:fcl]") ("collisions", po::value(), "display the collision shapes for task [name]") - ("controller,c", po::value()->default_value("../etc/icub/pos_tracker.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/icub/pos_tracker.yaml]") + ("controller,c", po::value()->default_value("../etc/icub/humanoid_pos_tracker.yaml"), "Configuration file of the tasks (yaml) [default: ../etc/icub/pos_tracker.yaml]") ("duration,d", po::value()->default_value(20), "duration in seconds [20]") ("enforce_position,e", po::value()->default_value(true), "enforce the positions of the URDF [default:true]") ("fast,f", "fast (simplified) Talos [default: false]") diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e4a35115..c647fe24 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -2,9 +2,19 @@ find_package(Boost COMPONENTS program_options) set(THREADS_PREFER_PTHREAD_FLAG ON) find_package(Threads REQUIRED) - set(BINARY ${CMAKE_PROJECT_NAME}_test) +include(ProcessorCount) +ProcessorCount(N) +if(NOT N EQUAL 0) + set(NUM_CORES ${N}) +endif() + +find_program (BASH_PROGRAM bash) +if (BASH_PROGRAM) + add_test (test_example_project ${BASH_PROGRAM} ${CMAKE_CURRENT_SOURCE_DIR}/test_example_project.sh ${CMAKE_C_COMPILER} ${CMAKE_CXX_COMPILER} ${CMAKE_CXX_STANDARD} ${CMAKE_CXX_FLAGS} ${CMAKE_PREFIX_PATH} ${CMAKE_INSTALL_PREFIX} ${NUM_CORES}) +endif (BASH_PROGRAM) + # the tests require Dart to run if(${COMPILE_ROBOT_DART_EXAMPLE} STREQUAL "ON") find_package(RobotDART REQUIRED COMPONENTS Simu OPTIONAL_COMPONENTS Magnum) @@ -23,16 +33,3 @@ foreach(TEST ${TEST_SOURCES}) target_link_libraries(${TEST_NAME}_graphics PUBLIC inria_wbc RobotDART::Magnum RobotDART::Simu Boost::program_options Threads::Threads) endif() endforeach() - - -include(ProcessorCount) -ProcessorCount(N) -if(NOT N EQUAL 0) - set(NUM_CORES ${N}) -endif() - -find_program (BASH_PROGRAM bash) -if (BASH_PROGRAM) - add_test (test_example_project ${BASH_PROGRAM} ${CMAKE_CURRENT_SOURCE_DIR}/test_example_project.sh ${CMAKE_PREFIX_PATH} ${CMAKE_INSTALL_PREFIX} ${NUM_CORES}) - -endif (BASH_PROGRAM) diff --git a/tests/test_example_project.sh b/tests/test_example_project.sh old mode 100644 new mode 100755 index ff1edb6c..119e96a1 --- a/tests/test_example_project.sh +++ b/tests/test_example_project.sh @@ -12,8 +12,8 @@ cd /tmp/example_project sh ./create_project.sh testc mkdir build cd build -cmake .. -DCMAKE_PREFIX_PATH=$1 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$2 -make -j$3 +cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_C_COMPILER=$1 -DCMAKE_CXX_COMPILER=$2 -DCMAKE_CXX_STANDARD=$3 -DCMAKE_CXX_FLAGS=$4 -DCMAKE_PREFIX_PATH=$5 -DCMAKE_INSTALL_PREFIX=$6 .. +make -j$7 From efc485f9e7f82b341bd89f3e30ce692cec342826 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Thu, 2 Jun 2022 16:01:13 +0200 Subject: [PATCH 35/46] add a new behavior to move the CoM --- CMakeLists.txt | 1 + etc/talos/move_com.yaml | 7 ++ .../inria_wbc/behaviors/humanoid/move_com.hpp | 32 ++++++++++ src/behaviors/humanoid/move_com.cpp | 64 +++++++++++++++++++ 4 files changed, 104 insertions(+) create mode 100644 etc/talos/move_com.yaml create mode 100644 include/inria_wbc/behaviors/humanoid/move_com.hpp create mode 100644 src/behaviors/humanoid/move_com.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 77fde983..db517a15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ set(INRIA_WBC_SOURCES src/behaviors/generic/cartesian.cpp src/behaviors/generic/cartesian_traj.cpp src/behaviors/humanoid/squat.cpp + src/behaviors/humanoid/move_com.cpp src/behaviors/humanoid/walk_on_spot.cpp src/behaviors/humanoid/walk.cpp src/behaviors/humanoid/clapping.cpp diff --git a/etc/talos/move_com.yaml b/etc/talos/move_com.yaml new file mode 100644 index 00000000..ba2d9ced --- /dev/null +++ b/etc/talos/move_com.yaml @@ -0,0 +1,7 @@ +BEHAVIOR: + name: humanoid::move_com + trajectory_duration: 2 + targets: [[0, 0, -0.2]] + mask: 001 + absolute: false + loop: true \ No newline at end of file diff --git a/include/inria_wbc/behaviors/humanoid/move_com.hpp b/include/inria_wbc/behaviors/humanoid/move_com.hpp new file mode 100644 index 00000000..7a15745d --- /dev/null +++ b/include/inria_wbc/behaviors/humanoid/move_com.hpp @@ -0,0 +1,32 @@ +#ifndef IWBC_BEHAVIOR_HUMANOID_MOVE_COM_HPP +#define IWBC_BEHAVIOR_HUMANOID_MOVE_COM_HPP +#include + +#include +#include +#include + +namespace inria_wbc { + namespace behaviors { + namespace humanoid { + class MoveCom : public Behavior { + public: + MoveCom(const controller_ptr_t& controller, const YAML::Node& config); + MoveCom() = delete; + MoveCom(const MoveCom&) = delete; + virtual ~MoveCom() {} + + void update(const controllers::SensorData& sensor_data = {}) override; + std::string behavior_type() const override { return controllers::behavior_types::DOUBLE_SUPPORT; }; + + private: + bool loop_ = false; + int time_ = 0; + std::vector trajectory_; + std::vector trajectory_d_; + std::vector trajectory_dd_; + }; + } // namespace humanoid + } // namespace behaviors +} // namespace inria_wbc +#endif \ No newline at end of file diff --git a/src/behaviors/humanoid/move_com.cpp b/src/behaviors/humanoid/move_com.cpp new file mode 100644 index 00000000..74ca8418 --- /dev/null +++ b/src/behaviors/humanoid/move_com.cpp @@ -0,0 +1,64 @@ +#include "inria_wbc/behaviors/humanoid/move_com.hpp" + +namespace inria_wbc { + namespace behaviors { + namespace humanoid { + static Register __talos_move_com("humanoid::move_com"); + + MoveCom::MoveCom(const controller_ptr_t& controller, const YAML::Node& config) : Behavior(controller, config) + { + IWBC_ASSERT(std::dynamic_pointer_cast(controller_) != 0, "Need a PosTracker for MovCom"); + auto c = IWBC_CHECK(config["BEHAVIOR"]); + float trajectory_duration = IWBC_CHECK(c["trajectory_duration"].as()); + behavior_type_ = this->behavior_type(); + controller_->set_behavior_type(behavior_type_); + + loop_ = IWBC_CHECK(c["loop"].as()); + auto targets = IWBC_CHECK(c["targets"].as>>()); + auto mask = IWBC_CHECK(c["mask"].as()); + auto absolute = IWBC_CHECK(c["absolute"].as()); + IWBC_ASSERT(mask.size() == 3, "The mask for the CoM should be 3-dimensional"); + + + Eigen::VectorXd task_init = std::static_pointer_cast(controller_)->get_com_ref(); + if (loop_) + if (absolute) + targets.push_back({task_init(0), task_init(1), task_init(2)}); + else + targets.push_back({0., 0., 0.}); + + + Eigen::VectorXd start = task_init; + for (size_t i = 0; i < targets.size(); ++i) { + IWBC_ASSERT(targets[i].size() == 3, "references need to be 3-dimensional"); + Eigen::VectorXd end = task_init; + for (size_t j = 0; j < targets[i].size(); ++j) + if (mask[j] == '1') + end[j] = absolute ? targets[i][j] : targets[i][j] + task_init[j]; + auto traj = trajs::min_jerk_trajectory(start, end, controller_->dt(), trajectory_duration); + auto traj_d = trajs::min_jerk_trajectory(start, end, controller_->dt(), trajectory_duration); + auto traj_dd = trajs::min_jerk_trajectory(start, end, controller_->dt(), trajectory_duration); + trajectory_.insert(trajectory_.end(), traj.begin(), traj.end()); + trajectory_d_.insert(trajectory_d_.end(), traj_d.begin(), traj_d.end()); + trajectory_dd_.insert(trajectory_dd_.end(), traj_dd.begin(), traj_dd.end()); + start = end; + } + } + + void MoveCom::update(const controllers::SensorData& sensor_data) + { + tsid::trajectories::TrajectorySample sample_ref(3, 3); + sample_ref.setValue(trajectory_[time_]); + sample_ref.setDerivative(trajectory_d_[time_]); + sample_ref.setSecondDerivative(trajectory_dd_[time_]); + std::static_pointer_cast(controller_)->set_com_ref(sample_ref); + controller_->update(sensor_data); + time_++; + if (loop_) + time_ = time_ % trajectory_.size(); + else + time_ = std::min(time_, (int) trajectory_.size() - 1); + } + } // namespace humanoid + } // namespace behaviors +} // namespace inria_wbc \ No newline at end of file From 6dc863210baf3e81e7b37631fb55ff8c71f593b7 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Thu, 2 Jun 2022 16:11:27 +0200 Subject: [PATCH 36/46] remove the squat and replace it by move_com --- CMakeLists.txt | 1 - etc/icub/squat.yaml | 7 ++- etc/talos/squat.yaml | 9 ++- .../inria_wbc/behaviors/humanoid/squat.hpp | 35 ----------- src/behaviors/humanoid/squat.cpp | 59 ------------------- src/robot_dart/qp_timer_test.cpp | 2 +- src/robot_dart/tutorial_0.cpp | 4 +- src/robot_dart/tutorial_1.cpp | 4 +- 8 files changed, 16 insertions(+), 105 deletions(-) delete mode 100644 include/inria_wbc/behaviors/humanoid/squat.hpp delete mode 100644 src/behaviors/humanoid/squat.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index db517a15..495706e1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,6 @@ set(INRIA_WBC_SOURCES src/behaviors/behavior.cpp src/behaviors/generic/cartesian.cpp src/behaviors/generic/cartesian_traj.cpp - src/behaviors/humanoid/squat.cpp src/behaviors/humanoid/move_com.cpp src/behaviors/humanoid/walk_on_spot.cpp src/behaviors/humanoid/walk.cpp diff --git a/etc/icub/squat.yaml b/etc/icub/squat.yaml index 993e5080..9cabb8d7 100644 --- a/etc/icub/squat.yaml +++ b/etc/icub/squat.yaml @@ -1,4 +1,7 @@ BEHAVIOR: - name: humanoid::squat + name: humanoid::move_com trajectory_duration: 1.0 - motion_size: 0.1 + targets: [[0, 0, -0.1]] + mask: 001 + absolute: false + loop: true \ No newline at end of file diff --git a/etc/talos/squat.yaml b/etc/talos/squat.yaml index 7e91e7c9..ba2d9ced 100644 --- a/etc/talos/squat.yaml +++ b/etc/talos/squat.yaml @@ -1,4 +1,7 @@ BEHAVIOR: - name: humanoid::squat - trajectory_duration: 6 - motion_size: 0.2 + name: humanoid::move_com + trajectory_duration: 2 + targets: [[0, 0, -0.2]] + mask: 001 + absolute: false + loop: true \ No newline at end of file diff --git a/include/inria_wbc/behaviors/humanoid/squat.hpp b/include/inria_wbc/behaviors/humanoid/squat.hpp deleted file mode 100644 index 3bb8abd7..00000000 --- a/include/inria_wbc/behaviors/humanoid/squat.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef IWBC_HUMANOID_SQUAT_HPP -#define IWBC_HUMANOID_SQUAT_HPP -#include -#include -#include - -#include -#include -#include - -namespace inria_wbc { - namespace behaviors { - namespace humanoid { - class Squat : public Behavior { - public: - Squat(const controller_ptr_t& controller, const YAML::Node& node); - Squat() = delete; - Squat(const Squat& other) = delete; - virtual ~Squat() {} - - void update(const controllers::SensorData& sensor_data = {}) override; - std::string behavior_type() const override { return controllers::behavior_types::DOUBLE_SUPPORT; }; - - private: - int time_ = 0; - int traj_selector_ = 0; - std::vector> trajectories_; - std::vector current_trajectory_; - float trajectory_duration_ = 3; //will be changed if specified in yaml - float motion_size_ = 0.2; //will be changed if specified in yaml - }; - } // namespace humanoid - } // namespace behaviors -} // namespace inria_wbc -#endif \ No newline at end of file diff --git a/src/behaviors/humanoid/squat.cpp b/src/behaviors/humanoid/squat.cpp deleted file mode 100644 index 2c99d276..00000000 --- a/src/behaviors/humanoid/squat.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include "inria_wbc/behaviors/humanoid/squat.hpp" - -namespace inria_wbc { - namespace behaviors { - namespace humanoid { - static Register _squat("humanoid::squat"); - - Squat::Squat(const controller_ptr_t& controller, const YAML::Node& config) : Behavior(controller, config) - { - auto pos_controller = std::dynamic_pointer_cast(controller_); - - if (!pos_controller) - IWBC_ERROR("Squat behavior requires at least a PosTracker controller."); - if (pos_controller->com_task()->getMask()(2) == 0) - IWBC_ERROR("Squat behavior requires mask of com task to be 1 on z-axis."); - - //////////////////// DEFINE COM TRAJECTORIES ////////////////////////////////////// - traj_selector_ = 0; - auto com_init = pos_controller->get_com_ref(); - - auto c = IWBC_CHECK(config["BEHAVIOR"]); - trajectory_duration_ = IWBC_CHECK(c["trajectory_duration"].as()); - motion_size_ = IWBC_CHECK(c["motion_size"].as()); - - behavior_type_ = this->behavior_type(); - pos_controller->set_behavior_type(behavior_type_); - - Eigen::Vector3d com_final = com_init; - com_final(2) -= motion_size_; - - trajectories_.push_back( - trajs::to_sample_trajectory( - trajs::min_jerk_trajectory(com_init, com_final, pos_controller->dt(), trajectory_duration_), - trajs::min_jerk_trajectory(com_init, com_final, pos_controller->dt(), trajectory_duration_), - trajs::min_jerk_trajectory(com_init, com_final, pos_controller->dt(), trajectory_duration_))); - trajectories_.push_back( - trajs::to_sample_trajectory( - trajs::min_jerk_trajectory(com_final, com_init, pos_controller->dt(), trajectory_duration_), - trajs::min_jerk_trajectory(com_final, com_init, pos_controller->dt(), trajectory_duration_), - trajs::min_jerk_trajectory(com_final, com_init, pos_controller->dt(), trajectory_duration_))); - current_trajectory_ = trajectories_[traj_selector_]; - } - - void Squat::update(const controllers::SensorData& sensor_data) - { - auto ref = current_trajectory_[time_]; - std::static_pointer_cast(controller_)->set_com_ref(ref); - - controller_->update(sensor_data); - time_++; - if (time_ == current_trajectory_.size()) { - time_ = 0; - traj_selector_ = ++traj_selector_ % trajectories_.size(); - current_trajectory_ = trajectories_[traj_selector_]; - } - } - } // namespace humanoid - } // namespace behaviors -} // namespace inria_wbc diff --git a/src/robot_dart/qp_timer_test.cpp b/src/robot_dart/qp_timer_test.cpp index a2807eaa..5df79f01 100644 --- a/src/robot_dart/qp_timer_test.cpp +++ b/src/robot_dart/qp_timer_test.cpp @@ -1,7 +1,7 @@ #include #include -#include "inria_wbc/behaviors/humanoid/squat.hpp" +#include "inria_wbc/behaviors/behavior.hpp" #include "inria_wbc/controllers/pos_tracker.hpp" #include "inria_wbc/exceptions.hpp" #include "inria_wbc/utils/timer.hpp" diff --git a/src/robot_dart/tutorial_0.cpp b/src/robot_dart/tutorial_0.cpp index c9debd82..3176dc6c 100644 --- a/src/robot_dart/tutorial_0.cpp +++ b/src/robot_dart/tutorial_0.cpp @@ -1,7 +1,7 @@ #include #include -#include "inria_wbc/behaviors/humanoid/squat.hpp" +#include "inria_wbc/behaviors/humanoid/move_com.hpp" #include "inria_wbc/controllers/pos_tracker.hpp" #include "inria_wbc/exceptions.hpp" @@ -28,7 +28,7 @@ int main(int argc, char* argv[]) //Now contruct the behavior, the behavior will send reference trajectories to the controller std::string behavior_conf_path = "../etc/talos/squat.yaml"; auto behavior_yaml = IWBC_CHECK(YAML::LoadFile(behavior_conf_path)); - auto behavior = std::make_shared(controller, behavior_yaml); + auto behavior = std::make_shared(controller, behavior_yaml); while (!stop) { behavior->update(); diff --git a/src/robot_dart/tutorial_1.cpp b/src/robot_dart/tutorial_1.cpp index ae428d52..4dfcd19e 100644 --- a/src/robot_dart/tutorial_1.cpp +++ b/src/robot_dart/tutorial_1.cpp @@ -1,7 +1,7 @@ #include #include -#include "inria_wbc/behaviors/humanoid/squat.hpp" +#include "inria_wbc/behaviors/humanoid/move_com.hpp" #include "inria_wbc/controllers/talos_pos_tracker.hpp" #include "inria_wbc/exceptions.hpp" #include "inria_wbc/robot_dart/cmd.hpp" @@ -38,7 +38,7 @@ int main(int argc, char* argv[]) //Now contruct the behavior, the behavior will send reference trajectories to the controller std::string behavior_conf_path = "../etc/talos/squat.yaml"; auto behavior_yaml = IWBC_CHECK(YAML::LoadFile(behavior_conf_path)); - auto behavior = std::make_shared(controller, behavior_yaml); + auto behavior = std::make_shared(controller, behavior_yaml); ////////// ROBOT_DART //////////////////////////////////////////////////////////////////// //Create talos robot From 8837ad61f74746df9be04b93cd399f525a4f7351 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Thu, 2 Jun 2022 16:28:30 +0200 Subject: [PATCH 37/46] partial fix of error for test_example_project.sh --- tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index c647fe24..bee5793c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -12,7 +12,7 @@ endif() find_program (BASH_PROGRAM bash) if (BASH_PROGRAM) - add_test (test_example_project ${BASH_PROGRAM} ${CMAKE_CURRENT_SOURCE_DIR}/test_example_project.sh ${CMAKE_C_COMPILER} ${CMAKE_CXX_COMPILER} ${CMAKE_CXX_STANDARD} ${CMAKE_CXX_FLAGS} ${CMAKE_PREFIX_PATH} ${CMAKE_INSTALL_PREFIX} ${NUM_CORES}) + add_test (test_example_project ${BASH_PROGRAM} ${CMAKE_CURRENT_SOURCE_DIR}/test_example_project.sh "${CMAKE_C_COMPILER}" "${CMAKE_CXX_COMPILER}" "${CMAKE_CXX_STANDARD}" "${CMAKE_CXX_FLAGS}" "${CMAKE_PREFIX_PATH}" "${CMAKE_INSTALL_PREFIX}" "${NUM_CORES}") endif (BASH_PROGRAM) # the tests require Dart to run From feb2629a64e68312d26b6333b91f34d7828e7811 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Thu, 2 Jun 2022 17:39:05 +0200 Subject: [PATCH 38/46] fix the error with arguments in test --- tests/CMakeLists.txt | 24 ++++++++++++++++++++++-- tests/test_example_project.sh | 2 +- 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index bee5793c..b0a97042 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -11,8 +11,28 @@ if(NOT N EQUAL 0) endif() find_program (BASH_PROGRAM bash) -if (BASH_PROGRAM) - add_test (test_example_project ${BASH_PROGRAM} ${CMAKE_CURRENT_SOURCE_DIR}/test_example_project.sh "${CMAKE_C_COMPILER}" "${CMAKE_CXX_COMPILER}" "${CMAKE_CXX_STANDARD}" "${CMAKE_CXX_FLAGS}" "${CMAKE_PREFIX_PATH}" "${CMAKE_INSTALL_PREFIX}" "${NUM_CORES}") +if (BASH_PROGRAM) + set(ARGS, "") + if (DEFINED CMAKE_C_COMPILER) + set(ARGS "${ARGS} -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER}") + endif() + if (DEFINED CMAKE_CXX_COMPILER) + set(ARGS "${ARGS} -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER}") + endif() + if (DEFINED CMAKE_CXX_STANDARD) + set(ARGS "${ARGS} -DCMAKE_CXX_STANDARD=${CMAKE_CXX_STANDARD}") + endif() + if (DEFINED CMAKE_CXX_FLAGS) + set(ARGS "${ARGS} -DCMAKE_CXX_FLAGS=${CMAKE_CXX_FLAGS}") + endif() + if (DEFINED CMAKE_PREFIX_PATH) + set(ARGS "${ARGS} -DCMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}") + endif() + if (DEFINED CMAKE_INSTALL_PREFIX) + set(ARGS "${ARGS} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}") + endif() + + add_test (test_example_project ${BASH_PROGRAM} ${CMAKE_CURRENT_SOURCE_DIR}/test_example_project.sh ${ARGS} "${NUM_CORES}") endif (BASH_PROGRAM) # the tests require Dart to run diff --git a/tests/test_example_project.sh b/tests/test_example_project.sh index 119e96a1..8aa0951f 100755 --- a/tests/test_example_project.sh +++ b/tests/test_example_project.sh @@ -12,7 +12,7 @@ cd /tmp/example_project sh ./create_project.sh testc mkdir build cd build -cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_C_COMPILER=$1 -DCMAKE_CXX_COMPILER=$2 -DCMAKE_CXX_STANDARD=$3 -DCMAKE_CXX_FLAGS=$4 -DCMAKE_PREFIX_PATH=$5 -DCMAKE_INSTALL_PREFIX=$6 .. +cmake -DCMAKE_BUILD_TYPE=Release ${@:1:${#}-1} .. make -j$7 From 263e8e57454894145366fa630982aac68292212f Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Thu, 2 Jun 2022 18:26:26 +0200 Subject: [PATCH 39/46] ok --- etc/talos/move_com.yaml | 4 +- .../etc/example_collisions_margin.yaml | 68 +++++++++++++++++++ .../etc/example_project_controller.yaml | 3 + .../inria_wbc/behaviors/ex_behavior.hpp | 2 +- example_project/src/robot_dart/tutorial_0.cpp | 4 +- example_project/src/robot_dart/tutorial_1.cpp | 4 +- 6 files changed, 78 insertions(+), 7 deletions(-) create mode 100644 example_project/etc/example_collisions_margin.yaml diff --git a/etc/talos/move_com.yaml b/etc/talos/move_com.yaml index ba2d9ced..85b2ffbe 100644 --- a/etc/talos/move_com.yaml +++ b/etc/talos/move_com.yaml @@ -1,7 +1,7 @@ BEHAVIOR: name: humanoid::move_com trajectory_duration: 2 - targets: [[0, 0, -0.2]] - mask: 001 + targets: [[-0.04, 0.0, 0.0],[0.04, 0.0, 0.0],[0.0, 0.0, 0.0]] + mask: 100 absolute: false loop: true \ No newline at end of file diff --git a/example_project/etc/example_collisions_margin.yaml b/example_project/etc/example_collisions_margin.yaml new file mode 100644 index 00000000..2ffa95c8 --- /dev/null +++ b/example_project/etc/example_collisions_margin.yaml @@ -0,0 +1,68 @@ +members: + leg_left: + leg_left_1_link: [[0.05,0,0.01,0.15],[-0.05,0,0.01,0.15]] + leg_left_2_link: [[0.05,0,0.01,0.15],[-0.05,0,0.01,0.15],[0,0.1,0.0,0.15]] + leg_left_3_link: [[0,0.15,0.0,0.15], + [0,0.015,-0.1,0.17],[0,0.13,-0.1,0.17], + [0,0.015,-0.2,0.17],[0,0.1,-0.2,0.17], + [0,0.05,-0.27,0.17], + [0,0.05,-0.37,0.17]] + leg_left_4_link: [[0,0.01,0.0,0.14],[0,0.1,0.0,0.14], + [0,0.01,-0.1,0.14],[0,0.1,-0.1,0.14], + [0,0.01,-0.2,0.14],[0,0.1,-0.2,0.14], + [0,-0.025,-0.3,0.1],[0,0.1,-0.32,0.12]] + leg_left_5_link: [[0,-0.01,0.0,0.14],[0,0.09,0.0,0.14]] + leg_left_6_link: [[0,-0.01,0.0,0.14],[0,0.09,0.0,0.14], + [-0.1,-0.01,0.0,0.11], + [0.06,-0.03,-0.08,0.1],[0.06,+0.03,-0.08,0.1], + [-0.02,-0.03,-0.08,0.1],[-0.02,+0.03,-0.08,0.1], + [-0.07,-0.03,-0.08,0.1],[-0.07,+0.03,-0.08,0.1]] + leg_right: + leg_right_1_link: [[0.05,0,0.01,0.15],[-0.05,0,0.01,0.15]] + leg_right_2_link: [[0.05,0,0.01,0.15],[-0.05,0,0.01,0.15],[0,-0.1,0.0,0.15]] + leg_right_3_link: [[0,-0.15,0.0,0.15], + [0,-0.015,-0.1,0.17],[0,-0.13,-0.1,0.17], + [0,-0.015,-0.2,0.17],[0,-0.1,-0.2,0.17], + [0,-0.05,-0.27,0.17], + [0,-0.05,-0.37,0.17]] + leg_right_4_link: [[0,-0.01,0.0,0.14],[0,-0.1,0.0,0.14], + [0,-0.01,-0.1,0.14],[0,-0.1,-0.1,0.14], + [0,-0.01,-0.2,0.14],[0,-0.1,-0.2,0.14], + [0,0.025,-0.3,0.1],[0,-0.1,-0.32,0.12]] + leg_right_5_link: [[0,0.01,0.0,0.14],[0,-0.09,0.0,0.14]] + leg_right_6_link: [[0,0.01,0.0,0.14],[0,-0.09,0.0,0.14], + [-0.1,0.01,0.0,0.11], + [0.06,0.03,-0.08,0.1],[0.06,-0.03,-0.08,0.1], + [-0.02,0.03,-0.08,0.1],[-0.02,-0.03,-0.08,0.1], + [-0.07,0.03,-0.08,0.1],[-0.07,-0.03,-0.08,0.1]] + arm_left: + arm_left_1_link: [[-0.03,0.14,0.04,0.13]] + arm_left_2_link: [[0.05,0.0,-0.02,0.13],[-0.05,0.0,-0.02,0.13], + [0.0,0.0,-0.1,0.15]] + arm_left_3_link: [[0.012,0.012,-0.2,0.19], + [0.025,0.025,-0.27,0.2]] + arm_left_4_link: [[0,0,0,0.2], + [-0.03,0.0,-0.05,0.15]] + arm_left_5_link: [[0.0,0.0,0.12,0.21],[0.0,0.0,0.05,0.21]] + arm_left_6_link: [[0,0,0,0.13]] + arm_left_7_link: [[0,0,0,0.15]] + gripper_left_base_link: [[0,0,-0.1,0.17],[0,0,-0.175,0.22]] + arm_right: + arm_right_1_link: [[-0.03,-0.14,0.04,0.13]] + arm_right_2_link: [[0.05,0.0,-0.02,0.13],[-0.05,0.0,-0.02,0.13], + [0.0,0.0,-0.1,0.15]] + arm_right_3_link: [[0.012,-0.012,-0.2,0.19], + [0.025,-0.025,-0.27,0.2]] + arm_right_4_link: [[0,0,0,0.2], + [-0.03,0.0,-0.05,0.18]] + arm_right_5_link: [[0.0,0.0,0.12,0.21],[0.0,0.0,0.05,0.21]] + arm_right_6_link: [[0,0,0,0.13]] + arm_right_7_link: [[0,0,0,0.15]] + gripper_right_base_link: [[0,0,-0.1,0.17],[0,0,-0.175,0.22]] + torso: + head_1_link: [[0,0,0.18,0.22]] + head_2_link: [[0,0,0.18,0.22]] + torso_2_link: [[0.0,0,0,0.2],[0.0,0.1,0.0,0.2],[0.0,-0.1,0.0,0.2],[-0.13,0,0,0.2],[-0.13,0.1,0.0,0.2],[-0.13,-0.1,0.0,0.2], + [0.0,0,0.125,0.23],[0.0,0.11,0.125,0.23],[0.0,-0.11,0.125,0.23],[-0.13,0,0.125,0.23],[-0.13,0.11,0.125,0.23],[-0.13,-0.11,0.125,0.23], + [0.0,0,0.28,0.23],[0.0,0.09,0.29,0.20],[0.0,-0.09,0.29,0.20],[-0.13,0,0.28,0.23],[-0.13,0.08,0.3,0.22],[-0.13,-0.08,0.3,0.22]] + base_link: [[-0.11,0,-0.05,0.25],[0,0,-0.05,0.25],[0,0.11,-0.05,0.25],[0,-0.11,-0.05,0.25],[-0.1,0.11,-0.05,0.25],[-0.1,-0.11,-0.05,0.25]] \ No newline at end of file diff --git a/example_project/etc/example_project_controller.yaml b/example_project/etc/example_project_controller.yaml index 1fe7a016..d20f03cf 100644 --- a/example_project/etc/example_project_controller.yaml +++ b/example_project/etc/example_project_controller.yaml @@ -1,5 +1,6 @@ CONTROLLER: name: ex-controller + solver: eiquadprog base_path: "/home/pal/inria_wbc/example_project/etc" tasks: tasks_example.yaml urdf: talos/talos.urdf @@ -14,6 +15,8 @@ CONTROLLER: # and our robot does not have a very good estimator yet closed_loop: false verbose: true + check_model_collisions: true + collision_path: example_collisions_margin.yaml mimic_dof_names : ["gripper_left_inner_double_joint", "gripper_left_fingertip_1_joint", "gripper_left_fingertip_2_joint", diff --git a/example_project/include/inria_wbc/behaviors/ex_behavior.hpp b/example_project/include/inria_wbc/behaviors/ex_behavior.hpp index 707d3076..4a6dcebc 100644 --- a/example_project/include/inria_wbc/behaviors/ex_behavior.hpp +++ b/example_project/include/inria_wbc/behaviors/ex_behavior.hpp @@ -16,7 +16,7 @@ namespace inria_wbc { ExBehavior() = delete; ExBehavior(const ExBehavior&) = delete; - void update(const controllers::SensorData& sensor_data) override; + void update(const controllers::SensorData& sensor_data ={}) override; virtual ~ExBehavior() {} std::string behavior_type() const override { return controllers::behavior_types::DOUBLE_SUPPORT; }; private: diff --git a/example_project/src/robot_dart/tutorial_0.cpp b/example_project/src/robot_dart/tutorial_0.cpp index f8f8465e..2abc4f24 100644 --- a/example_project/src/robot_dart/tutorial_0.cpp +++ b/example_project/src/robot_dart/tutorial_0.cpp @@ -1,7 +1,7 @@ #include #include -#include "inria_wbc/behaviors/humanoid/squat.hpp" +#include "inria_wbc/behaviors/ex_behavior.hpp" #include "inria_wbc/controllers/ex_controller.hpp" #include "inria_wbc/exceptions.hpp" @@ -27,7 +27,7 @@ int main(int argc, char* argv[]) //Now contruct the behavior, the behavior will send reference trajectories to the controller std::string behavior_conf_path = "../etc/@project_name@_behavior.yaml"; auto behavior_yaml = IWBC_CHECK(YAML::LoadFile(behavior_conf_path)); - auto behavior = std::make_shared(controller, behavior_yaml); + auto behavior = std::make_shared(controller, behavior_yaml); while (!stop) { behavior->update(); diff --git a/example_project/src/robot_dart/tutorial_1.cpp b/example_project/src/robot_dart/tutorial_1.cpp index e3c4834a..a823836f 100644 --- a/example_project/src/robot_dart/tutorial_1.cpp +++ b/example_project/src/robot_dart/tutorial_1.cpp @@ -1,7 +1,7 @@ #include #include -#include "inria_wbc/behaviors/humanoid/squat.hpp" +#include "inria_wbc/behaviors/ex_behavior.hpp" #include "inria_wbc/controllers/ex_controller.hpp" #include "inria_wbc/exceptions.hpp" #include "inria_wbc/robot_dart/cmd.hpp" @@ -37,7 +37,7 @@ int main(int argc, char* argv[]) //Now contruct the behavior, the behavior will send reference trajectories to the controller std::string behavior_conf_path = "../etc/@project_name@_behavior.yaml"; auto behavior_yaml = IWBC_CHECK(YAML::LoadFile(behavior_conf_path)); - auto behavior = std::make_shared(controller, behavior_yaml); + auto behavior = std::make_shared(controller, behavior_yaml); ////////// ROBOT_DART //////////////////////////////////////////////////////////////////// //Create talos robot From d17f916ecb203705c678f2e6bbff26b87589ac4f Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Fri, 3 Jun 2022 13:47:33 +0200 Subject: [PATCH 40/46] compute cop when data is available and stabilzer off --- src/controllers/humanoid_pos_tracker.cpp | 26 +++++++++++++++--------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/controllers/humanoid_pos_tracker.cpp b/src/controllers/humanoid_pos_tracker.cpp index c868298f..95ef191c 100644 --- a/src/controllers/humanoid_pos_tracker.cpp +++ b/src/controllers/humanoid_pos_tracker.cpp @@ -155,24 +155,30 @@ namespace inria_wbc { auto right_ankle_ref = get_full_se3_ref("rf"); auto torso_ref = get_full_se3_ref("torso"); - if (_use_stabilizer) { - IWBC_ASSERT(sensor_data.find("lf_force") != sensor_data.end(), "the stabilizer needs the LF force"); - IWBC_ASSERT(sensor_data.find("rf_force") != sensor_data.end(), "the stabilizer needs the RF force"); - IWBC_ASSERT(sensor_data.find("lf_torque") != sensor_data.end(), "the stabilizer needs the LF torque"); - IWBC_ASSERT(sensor_data.find("rf_torque") != sensor_data.end(), "the stabilizer needs the RF torque"); - IWBC_ASSERT(sensor_data.find("imu_vel") != sensor_data.end(), "the stabilizer imu needs angular velocity"); - + std::string left_ankle_name, right_ankle_name; + std::vector> cops; + + if (sensor_data.find("lf_force") != sensor_data.end() && sensor_data.find("rf_force") != sensor_data.end() + && sensor_data.find("lf_torque") != sensor_data.end() && sensor_data.find("rf_torque") != sensor_data.end()) { // we retrieve the tracked frame from the contact task as the frames have different names in different robots // the ankle = where is the f/t sensor - auto left_ankle_name = robot_->model().frames[contact("contact_lfoot")->getMotionTask().frame_id()].name; - auto right_ankle_name = robot_->model().frames[contact("contact_rfoot")->getMotionTask().frame_id()].name; + left_ankle_name = robot_->model().frames[contact("contact_lfoot")->getMotionTask().frame_id()].name; + right_ankle_name = robot_->model().frames[contact("contact_rfoot")->getMotionTask().frame_id()].name; // estimate the CoP / ZMP - auto cops = _cop_estimator.update(com_ref.getValue().head(2), + cops = _cop_estimator.update(com_ref.getValue().head(2), model_joint_pos(left_ankle_name).translation(), model_joint_pos(right_ankle_name).translation(), sensor_data.at("lf_torque"), sensor_data.at("lf_force"), sensor_data.at("rf_torque"), sensor_data.at("rf_force")); + } + + if (_use_stabilizer) { + IWBC_ASSERT(sensor_data.find("lf_force") != sensor_data.end(), "the stabilizer needs the LF force"); + IWBC_ASSERT(sensor_data.find("rf_force") != sensor_data.end(), "the stabilizer needs the RF force"); + IWBC_ASSERT(sensor_data.find("lf_torque") != sensor_data.end(), "the stabilizer needs the LF torque"); + IWBC_ASSERT(sensor_data.find("rf_torque") != sensor_data.end(), "the stabilizer needs the RF torque"); + IWBC_ASSERT(sensor_data.find("imu_vel") != sensor_data.end(), "the stabilizer imu needs angular velocity"); // if the foot is on the ground if (sensor_data.at("lf_force").norm() > _cop_estimator.fmin()) { From 91c70fe99782df38529e0ae1ac087b8632af7897 Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Tue, 14 Jun 2022 13:50:50 +0200 Subject: [PATCH 41/46] tests are ok --- .gitignore | 1 + etc/icub/tasks.yaml | 12 +- etc/talos/randomize.yaml | 157 ----------------------- etc/tiago/configurations-elbow-down.srdf | 1 - etc/tiago/configurations-elbow-up.srdf | 1 - tests/test_all_robots.cpp | 2 +- 6 files changed, 10 insertions(+), 164 deletions(-) delete mode 100644 etc/talos/randomize.yaml diff --git a/.gitignore b/.gitignore index 9caba06c..d7160252 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ build CMakeCache.txt CMakeFiles .vscode/ +etc/talos/randomize.yaml diff --git a/etc/icub/tasks.yaml b/etc/icub/tasks.yaml index ce72b05b..f6826f04 100644 --- a/etc/icub/tasks.yaml +++ b/etc/icub/tasks.yaml @@ -92,8 +92,10 @@ self_collision-left: l_knee: 0.08 r_knee: 0.08 weight: 500 - kp: 700.0 - p: 3 + kp: 50.0 + kd: 250.0 + margin: 0.02 + m: 0.2 self_collision-right: type: self-collision tracked: r_wrist_yaw @@ -108,5 +110,7 @@ self_collision-right: l_knee: 0.08 r_knee: 0.08 weight: 500 - kp: 700.0 - p: 3 \ No newline at end of file + kp: 50.0 + kd: 250.0 + margin: 0.02 + m: 0.2 \ No newline at end of file diff --git a/etc/talos/randomize.yaml b/etc/talos/randomize.yaml deleted file mode 100644 index ba5de749..00000000 --- a/etc/talos/randomize.yaml +++ /dev/null @@ -1,157 +0,0 @@ -head_yaw: - weight: 100.0 - type: se3 - tracked: head_2_joint - kp: 30.0 - mask: 000001 -head_pitch: - tracked: head_1_joint - weight: 100.0 - type: se3 - kp: 30.0 - mask: 000010 -head: - mask: 110000 - weight: 1.0 - kp: 1.0 - tracked: head_1_joint - type: se3 -rh: - weight: 10.0 - type: se3 - tracked: gripper_right_joint - kp: 30.0 - mask: 111111 -torso: - weight: 10.0 - type: se3 - tracked: torso_2_link - kp: 30.0 - mask: 000110 -lf: - weight: 1000.0 - type: se3 - tracked: leg_left_6_joint - kp: 30.0 - mask: 111111 -lh: - weight: 10.0 - tracked: gripper_left_joint - type: se3 - kp: 30.0 - mask: 111111 -com: - kp: 30.0 - type: com - weight: 1000.0 - mask: 111 -rf: - type: se3 - tracked: leg_right_6_joint - weight: 1000.0 - kp: 30.0 - mask: 111111 -posture: - type: posture - kp: 10.0 - weight: 1.75 - ref: inria_start -actuation_bounds: - type: actuation-bounds - weight: 10000 -momentum: - weight: 1000.0 - kp: 30.0 - type: momentum - mask: 000110 -bounds: - weight: 10000 - type: bounds -contact_lfoot: - lxp: 0.1 - kp: 30.0 - type: contact - joint: leg_left_6_joint - lyn: 0.069 - mu: 0.3 - lyp: 0.069 - lxn: 0.11 - lz: 0.107 - fmin: 5.0 - fmax: 1500.0 - normal: [0, 0, 1] -contact_rfoot: - normal: [0, 0, 1] - joint: leg_right_6_joint - kp: 30.0 - lz: 0.107 - lxn: 0.11 - type: contact - lxp: 0.1 - lyp: 0.069 - lyn: 0.069 - fmax: 1500.0 - fmin: 5.0 - mu: 0.3 -self_collision-left: - tracked: gripper_left_joint - avoided: - v_base_link_right: 0.115 - leg_right_4_joint: 0.1 - v_leg_left_3: 0.15 - v_leg_right_3: 0.15 - gripper_right_joint: 0.1 - arm_right_5_joint: 0.05 - base_link: 0.15 - torso_2_joint: 0.1 - leg_right_1_joint: 0.15 - leg_left_1_joint: 0.15 - leg_left_4_joint: 0.1 - v_base_link_left: 0.115 - type: self-collision - radius: 0.1 - weight: 500 - p: 3 - kp: 700.0 -self_collision-right: - type: self-collision - weight: 500 - tracked: gripper_right_joint - radius: 0.1 - avoided: - gripper_left_joint: 0.1 - arm_left_5_joint: 0.05 - v_leg_right_3: 0.15 - leg_right_1_joint: 0.15 - torso_2_joint: 0.1 - base_link: 0.15 - v_base_link_right: 0.115 - v_base_link_left: 0.115 - v_leg_left_3: 0.15 - leg_left_1_joint: 0.15 - leg_left_4_joint: 0.1 - leg_right_4_joint: 0.1 - p: 3 - kp: 700.0 -self_collision-arm-right: - type: self-collision - radius: 0.15 - avoided: - v_base_link_left: 0.115 - base_link: 0.15 - v_base_link_right: 0.115 - tracked: arm_right_4_joint - kp: 10.0 - p: 3 - weight: 10 -self_collision-arm-left: - avoided: - v_base_link_right: 0.115 - v_base_link_left: 0.115 - base_link: 0.15 - tracked: arm_left_4_joint - kp: 10.0 - radius: 0.15 - type: self-collision - weight: 10 - p: 3 \ No newline at end of file diff --git a/etc/tiago/configurations-elbow-down.srdf b/etc/tiago/configurations-elbow-down.srdf index 8dbb1160..76c09580 100644 --- a/etc/tiago/configurations-elbow-down.srdf +++ b/etc/tiago/configurations-elbow-down.srdf @@ -3,7 +3,6 @@ - diff --git a/etc/tiago/configurations-elbow-up.srdf b/etc/tiago/configurations-elbow-up.srdf index 07e090f6..0372792e 100644 --- a/etc/tiago/configurations-elbow-up.srdf +++ b/etc/tiago/configurations-elbow-up.srdf @@ -3,7 +3,6 @@ - diff --git a/tests/test_all_robots.cpp b/tests/test_all_robots.cpp index 638c52ee..5c0c5885 100644 --- a/tests/test_all_robots.cpp +++ b/tests/test_all_robots.cpp @@ -326,7 +326,7 @@ int main(int argc, char** argv) std::string name = "tiago"; for (auto& behavior_path : behaviors) { auto test1 = utest::make_test("[" + name + "] " + behavior_path); - auto tiago = std::make_shared(); + auto tiago = std::make_shared(1000, "tiago/tiago_steel_no_wheel.urdf"); bool expect_collision = false; UTEST_REGISTER(test_suite, test1, test_behavior(test1, base_path, controller_path, behavior_path, tiago, sensor_data_tiago, expect_collision, verbose)); } From ad15080895dec03077957442c9f3a4f9b3ce1028 Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Fri, 22 Jul 2022 15:12:07 +0200 Subject: [PATCH 42/46] updated example_project --- example_project/etc/frames_example.yaml | 8 +- example_project/etc/tasks_example.yaml | 104 +++++++++++++++++------ example_project/src/robot_dart/talos.cpp | 86 ++++++++++++++++--- 3 files changed, 160 insertions(+), 38 deletions(-) diff --git a/example_project/etc/frames_example.yaml b/example_project/etc/frames_example.yaml index 70c565dd..d1c46e7c 100644 --- a/example_project/etc/frames_example.yaml +++ b/example_project/etc/frames_example.yaml @@ -10,4 +10,10 @@ v_base_link_left: pos: [0.0, -0.1, 0] v_base_link_right: ref: "base_link" - pos: [0, 0.1, 0] \ No newline at end of file + pos: [0, 0.1, 0] +v_right_gripper: + ref: "gripper_right_base_link" + pos: [0., 0., -0.09] +v_left_gripper: + ref: "gripper_left_base_link" + pos: [0., 0., -0.09] diff --git a/example_project/etc/tasks_example.yaml b/example_project/etc/tasks_example.yaml index fae8a981..4bd171a2 100644 --- a/example_project/etc/tasks_example.yaml +++ b/example_project/etc/tasks_example.yaml @@ -1,3 +1,21 @@ +head: + type: se3 + tracked: head_1_joint + weight: 1.0 + kp: 1.0 + mask: 110000 +head_pitch: + type: se3 + tracked: head_1_joint + weight: 100.0 + kp: 30.0 + mask: 000010 +head_yaw: + type: se3 + tracked: head_2_joint + weight: 100.0 + kp: 30.0 + mask: 000001 lh: type: se3 tracked: gripper_left_joint @@ -26,27 +44,29 @@ rf: type: se3 tracked: leg_right_6_joint weight: 1000.0 - kp: 30.0 + kp: 30.0 mask: 111111 com: type: com weight: 1000.0 kp: 30.0 mask: 111 -momentum: - type: momentum - weight: 0.0 - kp: 300.0 - mask: 111111 posture: type: posture weight: 1.75 kp: 10.0 ref: inria_start +momentum: #!!!!!WARNING IS DISZBLED FOR SINGLE SUPPORT cf parse_stabiliser !!!!!!!!!!! + type: momentum + weight: 1000.0 + kp: 30.0 + mask: 000110 bounds: type: bounds weight: 10000 - dt: 0.001 +actuation_bounds: + type: actuation-bounds + weight: 10000 contact_lfoot: type: contact joint: leg_left_6_joint @@ -77,11 +97,11 @@ self_collision-left: type: self-collision tracked: gripper_left_joint radius: 0.1 - avoided: + avoided: gripper_right_joint: 0.1 arm_right_5_joint: 0.05 v_leg_right_3: 0.15 - v_leg_left_3: 0.15 + v_leg_left_3: 0.15 torso_2_joint: 0.1 base_link: 0.15 v_base_link_left: 0.115 @@ -90,18 +110,20 @@ self_collision-left: leg_left_1_joint: 0.15 leg_right_4_joint: 0.1 leg_left_4_joint: 0.1 - weight: 50 - kp: 500.0 - p: 4 + weight: 2000 + kp: 50.0 + kd: 250.0 + margin: 0.02 + m: 0.2 self_collision-right: type: self-collision tracked: gripper_right_joint radius: 0.1 - avoided: + avoided: gripper_left_joint: 0.1 arm_left_5_joint: 0.05 v_leg_right_3: 0.15 - v_leg_left_3: 0.15 + v_leg_left_3: 0.15 torso_2_joint: 0.1 base_link: 0.15 v_base_link_left: 0.115 @@ -110,10 +132,12 @@ self_collision-right: leg_left_1_joint: 0.15 leg_right_4_joint: 0.1 leg_left_4_joint: 0.1 - weight: 50 - kp: 500.0 - p: 4 -self_collision-arm-right: + weight: 2000 + kp: 50.0 + kd: 250.0 + margin: 0.02 + m: 0.2 +self_collision-elbow-right: type: self-collision tracked: arm_right_4_joint radius: 0.15 @@ -121,10 +145,12 @@ self_collision-arm-right: base_link: 0.15 v_base_link_left: 0.115 v_base_link_right: 0.115 - weight: 10 - kp: 10.0 - p: 4 -self_collision-arm-left: + weight: 1000 + kp: 50.0 + kd: 250.0 + margin: 0.02 + m: 0.2 +self_collision-elbow-left: type: self-collision tracked: arm_left_4_joint radius: 0.15 @@ -132,6 +158,34 @@ self_collision-arm-left: base_link: 0.15 v_base_link_left: 0.115 v_base_link_right: 0.115 - weight: 10 - kp: 10.0 - p: 4 \ No newline at end of file + weight: 1000 + kp: 150.0 + kd: 250.0 + margin: 0.02 + m: 0.2 +self_collision-wrist-right: + type: self-collision + tracked: arm_right_5_joint + radius: 0.10 + avoided: + base_link: 0.15 + v_base_link_left: 0.115 + v_base_link_right: 0.115 + weight: 1000 + kp: 50.0 + kd: 250.0 + margin: 0.02 + m: 0.2 +self_collision-wrist-left: + type: self-collision + tracked: arm_left_5_joint + radius: 0.10 + avoided: + base_link: 0.15 + v_base_link_left: 0.115 + v_base_link_right: 0.115 + weight: 1000 + kp: 150.0 + kd: 250.0 + margin: 0.02 + m: 0.2 \ No newline at end of file diff --git a/example_project/src/robot_dart/talos.cpp b/example_project/src/robot_dart/talos.cpp index d2fc1cf4..8f134d35 100644 --- a/example_project/src/robot_dart/talos.cpp +++ b/example_project/src/robot_dart/talos.cpp @@ -1,10 +1,11 @@ #include -#include #include #include #include #include +#include +#include #include #include @@ -20,7 +21,6 @@ #include "inria_wbc/behaviors/behavior.hpp" #include "inria_wbc/controllers/pos_tracker.hpp" -#include "inria_wbc/controllers/talos_pos_tracker.hpp" #include "inria_wbc/exceptions.hpp" #include "inria_wbc/robot_dart/cmd.hpp" #include "inria_wbc/robot_dart/damages.hpp" @@ -32,6 +32,8 @@ #include "inria_wbc/utils/timer.hpp" #include "tsid/tasks/task-self-collision.hpp" +#include // Boost need to be always included after pinocchio & inria_wbc + static const std::string red = "\x1B[31m"; static const std::string rst = "\x1B[0m"; static const std::string bold = "\x1B[1m"; @@ -61,6 +63,7 @@ int main(int argc, char* argv[]) ("sim_freq", po::value()->default_value(1000), "set the simulation frequency") ("srdf,s", po::value()->default_value(0.0), "save the configuration at the specified time") ("ghost,g", "display the ghost (Pinocchio model)") + ("model_collisions",po::value()->default_value(false), "display pinocchio qp model collision spheres") ("closed_loop", "Close the loop with floating base position and joint positions; required for torque control [default: from YAML file]") ("help,h", "produce help message") ("height", po::value()->default_value(false), "print total feet force data to adjust height in config") @@ -240,10 +243,16 @@ int main(int argc, char* argv[]) simu->add_visual_robot(self_collision_spheres.back()); } } - std::vector> torque_sensors; - auto talos_tracker_controller = std::static_pointer_cast(controller); - for (const auto& joint : talos_tracker_controller->torque_sensor_joints()) { + std::vector> torque_sensors; + std::vector torque_collision_joints = { + "leg_left_1_joint", "leg_left_2_joint", "leg_left_3_joint", "leg_left_4_joint", "leg_left_5_joint", "leg_left_6_joint", + "leg_right_1_joint", "leg_right_2_joint", "leg_right_3_joint", "leg_right_4_joint", "leg_right_5_joint", "leg_right_6_joint", + "torso_1_joint", "torso_2_joint", + "arm_left_1_joint", "arm_left_2_joint", "arm_left_3_joint", "arm_left_4_joint", + "arm_right_1_joint", "arm_right_2_joint", "arm_right_3_joint", "arm_right_4_joint"}; + + for (const auto& joint : torque_collision_joints) { torque_sensors.push_back(simu->add_sensor(robot, joint, control_freq)); std::cerr << "Add joint torque sensor: " << joint << std::endl; } @@ -277,6 +286,10 @@ int main(int argc, char* argv[]) Eigen::VectorXd activated_joints = Eigen::VectorXd::Zero(active_dofs.size()); + bool init_model_sphere_collisions = false; + std::vector> spheres; + bool is_colliding = false; + while (simu->scheduler().next_time() < vm["duration"].as() && !simu->graphics()->done()) { if (vm["damage"].as()) { @@ -380,9 +393,9 @@ int main(int argc, char* argv[]) auto q = controller->q(false); timer.end("solver"); - auto q_no_mimic = controller->filter_cmd(q).tail(ncontrollable); //no fb + Eigen::VectorXd q_no_mimic = controller->filter_cmd(q).tail(ncontrollable); //no fb timer.begin("cmd"); - auto q_damaged = inria_wbc::robot_dart::filter_cmd(q_no_mimic, controllable_dofs, active_dofs_controllable); + Eigen::VectorXd q_damaged = inria_wbc::robot_dart::filter_cmd(q_no_mimic, controllable_dofs, active_dofs_controllable); if (vm["actuators"].as() == "velocity" || vm["actuators"].as() == "servo") cmd = inria_wbc::robot_dart::compute_velocities(robot, q_damaged, 1. / control_freq, active_dofs_controllable); @@ -395,11 +408,25 @@ int main(int argc, char* argv[]) } timer.end("cmd"); + Eigen::VectorXd translate_ghost = Eigen::VectorXd::Zero(6); if (ghost) { - Eigen::VectorXd translate_ghost = Eigen::VectorXd::Zero(6); translate_ghost(0) -= 1; - ghost->set_positions(controller->filter_cmd(q).tail(ncontrollable), controllable_dofs); - ghost->set_positions(q.head(6) + translate_ghost, floating_base); + ghost->set_positions(controller->filter_cmd(controller->q_solver(false)).tail(ncontrollable), controllable_dofs); + ghost->set_positions(controller->q_solver(false).head(6) + translate_ghost, floating_base); + } + + is_colliding = controller->is_model_colliding(); + if (vm["model_collisions"].as()) { + auto spherical_members = controller->collision_check().spherical_members(); + auto sphere_color = dart::Color::Green(0.5); + + if (init_model_sphere_collisions == false) { + spheres = inria_wbc::robot_dart::create_spherical_members(spherical_members, *simu, sphere_color); + init_model_sphere_collisions = true; + } + else { + inria_wbc::robot_dart::update_spherical_members(spherical_members, spheres, sphere_color, is_colliding, controller->collision_check().collision_index(), translate_ghost.head(3)); + } } } @@ -446,6 +473,21 @@ int main(int argc, char* argv[]) robot->set_commands(cmd, active_dofs_controllable); simu->step_world(); timer.end("sim"); + + // auto col = simu->world()->getConstraintSolver()->getLastCollisionResult(); + // size_t nc = col.getNumContacts(); + // size_t contact_count = 0; + // for (size_t i = 0; i < nc; i++) { + // auto& ct = col.getContact(i); + // auto f1 = ct.collisionObject1->getShapeFrame(); + // auto f2 = ct.collisionObject2->getShapeFrame(); + // std::string name1, name2; + // if (f1->isShapeNode()) + // name1 = f1->asShapeNode()->getBodyNodePtr()->getName(); + // if (f1->isShapeNode()) + // name2 = f1->asShapeNode()->getBodyNodePtr()->getName(); + // std::cout << "contact:" << name1<< " -- " << name2 << std::endl; + // } } if (traj_saver) @@ -456,10 +498,14 @@ int main(int argc, char* argv[]) timer.report(*x.second, simu->scheduler().current_time()); else if (x.first == "cmd") (*x.second) << cmd.transpose() << std::endl; + else if (x.first == "tau") + (*x.second) << controller->tau().transpose() << std::endl; else if (x.first == "com") // the real com (*x.second) << robot->com().transpose() << std::endl; else if (x.first == "controller_com") // the com according to controller (*x.second) << controller->com().transpose() << std::endl; + else if (x.first == "objective_value") + (*x.second) << controller_pos->objective_value() << std::endl; else if (x.first.find("cost_") != std::string::npos) // e.g. cost_com (*x.second) << controller->cost(x.first.substr(strlen("cost_"))) << std::endl; else if (x.first == "ft") @@ -507,8 +553,22 @@ int main(int argc, char* argv[]) else (*x.second) << Eigen::Vector2d::Constant(1000).transpose() << std::endl; } - else if (robot->body_node(x.first) != nullptr) - (*x.second) << robot->body_pose(x.first).translation().transpose() << std::endl; + else if (x.first.find("task_") != std::string::npos) // e.g. task_lh + { + auto ref = controller_pos->se3_task(x.first.substr(strlen("task_")))->getReference(); + (*x.second) << ref.getValue().transpose() << " " + << ref.getDerivative().transpose() << " " + << ref.getSecondDerivative().transpose() << std::endl; + } + else if (robot->body_node(x.first) != nullptr) { + pinocchio::SE3 frame; + frame.rotation() = robot->body_pose(x.first).rotation(); + frame.translation() = robot->body_pose(x.first).translation(); + + Eigen::VectorXd vec(12); + tsid::math::SE3ToVector(frame, vec); + (*x.second) << vec.transpose() << std::endl; + } } if (vm.count("srdf")) { auto conf = vm["srdf"].as(); @@ -520,6 +580,8 @@ int main(int argc, char* argv[]) #ifdef GRAPHIC // to avoid the warning oss.precision(3); timer.report(oss, simu->scheduler().current_time(), -1, '\n'); + if (is_colliding) + oss << "Model is colliding" << std::endl; if (!vm.count("mp4")) simu->set_text_panel(oss.str()); #endif From ac3234bc983be0b1498d5e6b62dcd6621c85a481 Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Fri, 22 Jul 2022 16:32:47 +0200 Subject: [PATCH 43/46] updated .md --- docs/installation.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/docs/installation.md b/docs/installation.md index 4598901c..86221420 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -14,6 +14,20 @@ mkdir install sudo apt-get install libyaml-cpp-dev ``` +## assimp 5.0 + +``` +git clone --depth 1 https://github.com/assimp/assimp.git +cd assimp +git checkout v5.0.0 +mkdir build +cd build +cmake .. +make -j$(nproc) +sudo make install +make clean +``` + ## pinocchio More information on pinnochio [here](https://github.com/stack-of-tasks/pinocchio) From 16fe261187096edcc6752dba0e95c127fcc7b2c0 Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Fri, 22 Jul 2022 14:51:25 +0000 Subject: [PATCH 44/46] switched to map --- docs/installation.md | 6 ++++++ include/inria_wbc/utils/robot_model.hpp | 2 +- src/utils/robot_model.cpp | 2 +- tests/test_robot_model.cpp | 2 +- 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/docs/installation.md b/docs/installation.md index 86221420..085b99f4 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -14,6 +14,12 @@ mkdir install sudo apt-get install libyaml-cpp-dev ``` +## boost-program-options-dev + +``` +sudo apt-get install libboost-program-options-dev +``` + ## assimp 5.0 ``` diff --git a/include/inria_wbc/utils/robot_model.hpp b/include/inria_wbc/utils/robot_model.hpp index 48456e97..342aff5b 100644 --- a/include/inria_wbc/utils/robot_model.hpp +++ b/include/inria_wbc/utils/robot_model.hpp @@ -63,7 +63,7 @@ namespace utils { //Compute torques from sensor_data external forces. //It is here done for the two foot contacts but it could be generalized to less or more contacts if needed //You could directly use pinocchio rnea if there is no external forces or no foot mass to add - Eigen::VectorXd compute_rnea_double_support(const std::unordered_map& sensor_data, + Eigen::VectorXd compute_rnea_double_support(const std::map& sensor_data, const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Eigen::VectorXd& a, diff --git a/src/utils/robot_model.cpp b/src/utils/robot_model.cpp index bca8e4fe..8fb40813 100644 --- a/src/utils/robot_model.cpp +++ b/src/utils/robot_model.cpp @@ -135,7 +135,7 @@ namespace utils { return pinocchio::getJointKinematicHessian(_model, _data, id, reference_frame); } - Eigen::VectorXd RobotModel::compute_rnea_double_support(const std::unordered_map& sensor_data, + Eigen::VectorXd RobotModel::compute_rnea_double_support(const std::map& sensor_data, const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Eigen::VectorXd& a, diff --git a/tests/test_robot_model.cpp b/tests/test_robot_model.cpp index fd56e57a..30039416 100644 --- a/tests/test_robot_model.cpp +++ b/tests/test_robot_model.cpp @@ -65,7 +65,7 @@ void test_compute_rnea(utest::test_t test, inria_wbc::utils::RobotModel robot_mo robot_model.update(q, dq, ddq, true, true); - std::unordered_map sensor_data; + std::map sensor_data; Eigen::Vector3d data = Eigen::Vector3d::Zero(); sensor_data["lf_torque"] = data; sensor_data["rf_torque"] = data; From 95804ff341cba0c34a6162ea781a4598037670a3 Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Thu, 8 Sep 2022 17:51:07 +0200 Subject: [PATCH 45/46] move_feet --- CMakeLists.txt | 1 + etc/talos/move_feet.yaml | 9 ++ .../behaviors/humanoid/move_feet.hpp | 39 +++++++ include/inria_wbc/controllers/pos_tracker.hpp | 1 + src/behaviors/humanoid/move_feet.cpp | 105 ++++++++++++++++++ src/controllers/pos_tracker.cpp | 9 +- 6 files changed, 163 insertions(+), 1 deletion(-) create mode 100644 etc/talos/move_feet.yaml create mode 100644 include/inria_wbc/behaviors/humanoid/move_feet.hpp create mode 100644 src/behaviors/humanoid/move_feet.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 495706e1..57cdc4b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ set(INRIA_WBC_SOURCES src/behaviors/generic/cartesian.cpp src/behaviors/generic/cartesian_traj.cpp src/behaviors/humanoid/move_com.cpp + src/behaviors/humanoid/move_feet.cpp src/behaviors/humanoid/walk_on_spot.cpp src/behaviors/humanoid/walk.cpp src/behaviors/humanoid/clapping.cpp diff --git a/etc/talos/move_feet.yaml b/etc/talos/move_feet.yaml new file mode 100644 index 00000000..8b992f20 --- /dev/null +++ b/etc/talos/move_feet.yaml @@ -0,0 +1,9 @@ +BEHAVIOR: + name : generic::cartesian + task_names: [lf, rf] + contact_names: [contact_lfoot, contact_rfoot] + relative_targets_pos: [[0.0, 0.0, 1], [0, 0, 1]] # empty sublist for no target es. [[1,2,0], []] + relative_targets_rpy: [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # empty sublist for no target es. [[], [3.14,0,0]] + trajectory_duration: 4 + loop: false + \ No newline at end of file diff --git a/include/inria_wbc/behaviors/humanoid/move_feet.hpp b/include/inria_wbc/behaviors/humanoid/move_feet.hpp new file mode 100644 index 00000000..170c2f6d --- /dev/null +++ b/include/inria_wbc/behaviors/humanoid/move_feet.hpp @@ -0,0 +1,39 @@ +#ifndef IWBC_BEHAVIOR_GENERIC_MoveFeet_HPP +#define IWBC_BEHAVIOR_GENERIC_MoveFeet_HPP +#include +#include +#include + +#include +#include +#include + +namespace inria_wbc { + namespace behaviors { + namespace generic { + class MoveFeet : public Behavior { + public: + MoveFeet(const controller_ptr_t& controller, const YAML::Node& config); + MoveFeet() = delete; + MoveFeet(const MoveFeet&) = delete; + virtual ~MoveFeet() {} + + void update(const controllers::SensorData& sensor_data = {}) override; + std::string behavior_type() const override { return controllers::behavior_types::DOUBLE_SUPPORT; }; + + private: + int time_ = 0; + int traj_selector_ = 0; + std::vector>> trajectories_; + std::vector>> trajectories_d_; + std::vector>> trajectories_dd_; + + float trajectory_duration_; // from YAML + std::vector task_names_; // from YAML + std::vector contact_names_; // from YAML + bool loop_; // from YAML + }; + } // namespace generic + } // namespace behaviors +} // namespace inria_wbc +#endif \ No newline at end of file diff --git a/include/inria_wbc/controllers/pos_tracker.hpp b/include/inria_wbc/controllers/pos_tracker.hpp index f5ea59a5..8bc21bb8 100644 --- a/include/inria_wbc/controllers/pos_tracker.hpp +++ b/include/inria_wbc/controllers/pos_tracker.hpp @@ -55,6 +55,7 @@ namespace inria_wbc { void set_se3_ref(const pinocchio::SE3& ref, const std::string& task_name); void set_contact_se3_ref(const pinocchio::SE3& ref, const std::string& contact_name); void set_se3_ref(tsid::trajectories::TrajectorySample& sample, const std::string& task_name); + void set_contact_se3_ref(tsid::trajectories::TrajectorySample& sample, const std::string& contact_name); void remove_contact(const std::string& contact_name); void add_contact(const std::string& contact_name); diff --git a/src/behaviors/humanoid/move_feet.cpp b/src/behaviors/humanoid/move_feet.cpp new file mode 100644 index 00000000..d43a2f2a --- /dev/null +++ b/src/behaviors/humanoid/move_feet.cpp @@ -0,0 +1,105 @@ +#include "inria_wbc/behaviors/humanoid/move_feet.hpp" + +namespace inria_wbc { + namespace behaviors { + namespace generic { + static Register __talos_move_arm("humanoid::move-feet"); + + MoveFeet::MoveFeet(const controller_ptr_t& controller, const YAML::Node& config) : Behavior(controller, config), + traj_selector_(0) + { + + auto c = IWBC_CHECK(config["BEHAVIOR"]); + trajectory_duration_ = IWBC_CHECK(c["trajectory_duration"].as()); + behavior_type_ = this->behavior_type(); + controller_->set_behavior_type(behavior_type_); + + loop_ = IWBC_CHECK(c["loop"].as()); + task_names_ = IWBC_CHECK(c["task_names"].as>()); + contact_names_ = IWBC_CHECK(c["contact_names"].as>()); + + for (auto& task_name : task_names_) { + IWBC_ASSERT(std::static_pointer_cast(controller_)->has_task(task_name), "active_walk: a " + task_name + " task is required"); + } + + for (auto& contact_name : contact_names_) { + IWBC_ASSERT(std::static_pointer_cast(controller_)->has_contact(contact_name), "active_walk: a " + contact_name + " task is required"); + } + + auto ts = IWBC_CHECK(c["relative_targets_pos"].as>>()); + auto to = IWBC_CHECK(c["relative_targets_rpy"].as>>()); + + if (task_names_.size() != ts.size()) + IWBC_ERROR("MoveFeet behavior needs the same number of tasks and targets"); + + for (uint i = 0; i < task_names_.size(); i++) { + + auto task_init = std::static_pointer_cast(controller_)->get_se3_ref(task_names_[i]); + auto task_final = task_init; + + if (ts[i].size() == 3) + task_final.translation() = Eigen::Vector3d::Map(ts[i].data()) + task_init.translation(); + + if (to[i].size() == 3) { + Eigen::Matrix3d rot = (Eigen::AngleAxisd(to[i][2], Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(to[i][1], Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(to[i][0], Eigen::Vector3d::UnitX())) + .toRotationMatrix(); + + task_final.rotation() = rot * task_init.rotation(); + } + + std::vector> trajectory; + std::vector> trajectory_d; + std::vector> trajectory_dd; + + trajectory.push_back(trajs::min_jerk_trajectory(task_init, task_final, controller_->dt(), trajectory_duration_)); + trajectory_d.push_back(trajs::min_jerk_trajectory(task_init, task_final, controller_->dt(), trajectory_duration_)); + trajectory_dd.push_back(trajs::min_jerk_trajectory(task_init, task_final, controller_->dt(), trajectory_duration_)); + + if (loop_) { + trajectory.push_back(trajs::min_jerk_trajectory(task_final, task_init, controller_->dt(), trajectory_duration_)); + trajectory_d.push_back(trajs::min_jerk_trajectory(task_final, task_init, controller_->dt(), trajectory_duration_)); + trajectory_dd.push_back(trajs::min_jerk_trajectory(task_final, task_init, controller_->dt(), trajectory_duration_)); + } + + trajectories_.push_back(trajectory); + trajectories_d_.push_back(trajectory_d); + trajectories_dd_.push_back(trajectory_dd); + } + } + + void MoveFeet::update(const controllers::SensorData& sensor_data) + { + + for (uint i = 0; i < task_names_.size(); i++) { + if (traj_selector_ < trajectories_[i].size()) { + + auto ref = trajectories_[i][traj_selector_][time_]; + Eigen::VectorXd ref_vec(12); + tsid::math::SE3ToVector(ref, ref_vec); + + tsid::trajectories::TrajectorySample sample_ref(12, 6); + sample_ref.setValue(ref_vec); + sample_ref.setDerivative(trajectories_d_[i][traj_selector_][time_]); + sample_ref.setSecondDerivative(trajectories_dd_[i][traj_selector_][time_]); + + std::static_pointer_cast(controller_)->set_se3_ref(sample_ref, task_names_[i]); + std::static_pointer_cast(controller_)->set_contact_se3_ref(sample_ref, contact_names_[i]); + } + } + + controller_->update(sensor_data); + ++time_; + if (trajectories_.size() > 0) { + if (time_ == trajectories_[0][traj_selector_].size()) { + time_ = 0; + traj_selector_ = ++traj_selector_; + if (loop_) + traj_selector_ = traj_selector_ % trajectories_[0].size(); + } + } + } + } // namespace generic + } // namespace behaviors +} // namespace inria_wbc \ No newline at end of file diff --git a/src/controllers/pos_tracker.cpp b/src/controllers/pos_tracker.cpp index be2e9713..14cda7a2 100644 --- a/src/controllers/pos_tracker.cpp +++ b/src/controllers/pos_tracker.cpp @@ -237,6 +237,12 @@ namespace inria_wbc { task->setReference(sample); } + void PosTracker::set_contact_se3_ref(tsid::trajectories::TrajectorySample& sample, const std::string& contact_name) + { + auto c = std::dynamic_pointer_cast(contact(contact_name)); + c->setReference(sample); + } + void PosTracker::remove_contact(const std::string& contact_name) { if (verbose_) @@ -278,7 +284,8 @@ namespace inria_wbc { auto foot_mass = robot_->model().inertias[robot_->model().getJointId(ankle_frame)].mass(); // auto contact_normal = contact(contact_name)->getContactNormal(); - force_tsid.head(3) += foot_mass * robot_->model().gravity.linear();; + force_tsid.head(3) += foot_mass * robot_->model().gravity.linear(); + ; Eigen::Vector3d tmp = force_tsid.head(3); force_tsid.tail(3) -= (ankle_world.translation() - sole_world.translation()).cross(tmp); } From 3a8dffee06281e1e790a410915e6652c103aa9dc Mon Sep 17 00:00:00 2001 From: eloise dalin Date: Fri, 9 Sep 2022 10:51:44 +0200 Subject: [PATCH 46/46] small fix --- etc/talos/move_feet.yaml | 4 ++-- src/behaviors/humanoid/move_feet.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/etc/talos/move_feet.yaml b/etc/talos/move_feet.yaml index 8b992f20..59fa68ba 100644 --- a/etc/talos/move_feet.yaml +++ b/etc/talos/move_feet.yaml @@ -1,8 +1,8 @@ BEHAVIOR: - name : generic::cartesian + name : humanoid::move-feet task_names: [lf, rf] contact_names: [contact_lfoot, contact_rfoot] - relative_targets_pos: [[0.0, 0.0, 1], [0, 0, 1]] # empty sublist for no target es. [[1,2,0], []] + relative_targets_pos: [[0.05, 0.05, 0.05], [0.05, 0.05, 0.05]] # empty sublist for no target es. [[1,2,0], []] relative_targets_rpy: [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # empty sublist for no target es. [[], [3.14,0,0]] trajectory_duration: 4 loop: false diff --git a/src/behaviors/humanoid/move_feet.cpp b/src/behaviors/humanoid/move_feet.cpp index d43a2f2a..2e3b1f8d 100644 --- a/src/behaviors/humanoid/move_feet.cpp +++ b/src/behaviors/humanoid/move_feet.cpp @@ -3,7 +3,7 @@ namespace inria_wbc { namespace behaviors { namespace generic { - static Register __talos_move_arm("humanoid::move-feet"); + static Register __talos_move_feet("humanoid::move-feet"); MoveFeet::MoveFeet(const controller_ptr_t& controller, const YAML::Node& config) : Behavior(controller, config), traj_selector_(0)