Skip to content

Commit 210217e

Browse files
author
Eloïse Dalin
authored
Merge pull request #115 from resibots/move_feet
Move feet
2 parents 16fe261 + 3a8dffe commit 210217e

File tree

6 files changed

+163
-1
lines changed

6 files changed

+163
-1
lines changed

‎CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ set(INRIA_WBC_SOURCES
2828
src/behaviors/generic/cartesian.cpp
2929
src/behaviors/generic/cartesian_traj.cpp
3030
src/behaviors/humanoid/move_com.cpp
31+
src/behaviors/humanoid/move_feet.cpp
3132
src/behaviors/humanoid/walk_on_spot.cpp
3233
src/behaviors/humanoid/walk.cpp
3334
src/behaviors/humanoid/clapping.cpp

‎etc/talos/move_feet.yaml

+9
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
BEHAVIOR:
2+
name : humanoid::move-feet
3+
task_names: [lf, rf]
4+
contact_names: [contact_lfoot, contact_rfoot]
5+
relative_targets_pos: [[0.05, 0.05, 0.05], [0.05, 0.05, 0.05]] # empty sublist for no target es. [[1,2,0], []]
6+
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]]
7+
trajectory_duration: 4
8+
loop: false
9+

‎include/inria_wbc/behaviors/humanoid/move_feet.hpp

+39
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
#ifndef IWBC_BEHAVIOR_GENERIC_MoveFeet_HPP
2+
#define IWBC_BEHAVIOR_GENERIC_MoveFeet_HPP
3+
#include <chrono>
4+
#include <iostream>
5+
#include <signal.h>
6+
7+
#include <inria_wbc/behaviors/behavior.hpp>
8+
#include <inria_wbc/controllers/pos_tracker.hpp>
9+
#include <inria_wbc/trajs/trajectory_generator.hpp>
10+
11+
namespace inria_wbc {
12+
namespace behaviors {
13+
namespace generic {
14+
class MoveFeet : public Behavior {
15+
public:
16+
MoveFeet(const controller_ptr_t& controller, const YAML::Node& config);
17+
MoveFeet() = delete;
18+
MoveFeet(const MoveFeet&) = delete;
19+
virtual ~MoveFeet() {}
20+
21+
void update(const controllers::SensorData& sensor_data = {}) override;
22+
std::string behavior_type() const override { return controllers::behavior_types::DOUBLE_SUPPORT; };
23+
24+
private:
25+
int time_ = 0;
26+
int traj_selector_ = 0;
27+
std::vector<std::vector<std::vector<pinocchio::SE3>>> trajectories_;
28+
std::vector<std::vector<std::vector<Eigen::VectorXd>>> trajectories_d_;
29+
std::vector<std::vector<std::vector<Eigen::VectorXd>>> trajectories_dd_;
30+
31+
float trajectory_duration_; // from YAML
32+
std::vector<std::string> task_names_; // from YAML
33+
std::vector<std::string> contact_names_; // from YAML
34+
bool loop_; // from YAML
35+
};
36+
} // namespace generic
37+
} // namespace behaviors
38+
} // namespace inria_wbc
39+
#endif

‎include/inria_wbc/controllers/pos_tracker.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@ namespace inria_wbc {
5555
void set_se3_ref(const pinocchio::SE3& ref, const std::string& task_name);
5656
void set_contact_se3_ref(const pinocchio::SE3& ref, const std::string& contact_name);
5757
void set_se3_ref(tsid::trajectories::TrajectorySample& sample, const std::string& task_name);
58+
void set_contact_se3_ref(tsid::trajectories::TrajectorySample& sample, const std::string& contact_name);
5859

5960
void remove_contact(const std::string& contact_name);
6061
void add_contact(const std::string& contact_name);

‎src/behaviors/humanoid/move_feet.cpp

+105
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
#include "inria_wbc/behaviors/humanoid/move_feet.hpp"
2+
3+
namespace inria_wbc {
4+
namespace behaviors {
5+
namespace generic {
6+
static Register<MoveFeet> __talos_move_feet("humanoid::move-feet");
7+
8+
MoveFeet::MoveFeet(const controller_ptr_t& controller, const YAML::Node& config) : Behavior(controller, config),
9+
traj_selector_(0)
10+
{
11+
12+
auto c = IWBC_CHECK(config["BEHAVIOR"]);
13+
trajectory_duration_ = IWBC_CHECK(c["trajectory_duration"].as<float>());
14+
behavior_type_ = this->behavior_type();
15+
controller_->set_behavior_type(behavior_type_);
16+
17+
loop_ = IWBC_CHECK(c["loop"].as<bool>());
18+
task_names_ = IWBC_CHECK(c["task_names"].as<std::vector<std::string>>());
19+
contact_names_ = IWBC_CHECK(c["contact_names"].as<std::vector<std::string>>());
20+
21+
for (auto& task_name : task_names_) {
22+
IWBC_ASSERT(std::static_pointer_cast<inria_wbc::controllers::PosTracker>(controller_)->has_task(task_name), "active_walk: a " + task_name + " task is required");
23+
}
24+
25+
for (auto& contact_name : contact_names_) {
26+
IWBC_ASSERT(std::static_pointer_cast<inria_wbc::controllers::PosTracker>(controller_)->has_contact(contact_name), "active_walk: a " + contact_name + " task is required");
27+
}
28+
29+
auto ts = IWBC_CHECK(c["relative_targets_pos"].as<std::vector<std::vector<double>>>());
30+
auto to = IWBC_CHECK(c["relative_targets_rpy"].as<std::vector<std::vector<double>>>());
31+
32+
if (task_names_.size() != ts.size())
33+
IWBC_ERROR("MoveFeet behavior needs the same number of tasks and targets");
34+
35+
for (uint i = 0; i < task_names_.size(); i++) {
36+
37+
auto task_init = std::static_pointer_cast<inria_wbc::controllers::PosTracker>(controller_)->get_se3_ref(task_names_[i]);
38+
auto task_final = task_init;
39+
40+
if (ts[i].size() == 3)
41+
task_final.translation() = Eigen::Vector3d::Map(ts[i].data()) + task_init.translation();
42+
43+
if (to[i].size() == 3) {
44+
Eigen::Matrix3d rot = (Eigen::AngleAxisd(to[i][2], Eigen::Vector3d::UnitZ())
45+
* Eigen::AngleAxisd(to[i][1], Eigen::Vector3d::UnitY())
46+
* Eigen::AngleAxisd(to[i][0], Eigen::Vector3d::UnitX()))
47+
.toRotationMatrix();
48+
49+
task_final.rotation() = rot * task_init.rotation();
50+
}
51+
52+
std::vector<std::vector<pinocchio::SE3>> trajectory;
53+
std::vector<std::vector<Eigen::VectorXd>> trajectory_d;
54+
std::vector<std::vector<Eigen::VectorXd>> trajectory_dd;
55+
56+
trajectory.push_back(trajs::min_jerk_trajectory(task_init, task_final, controller_->dt(), trajectory_duration_));
57+
trajectory_d.push_back(trajs::min_jerk_trajectory<trajs::d_order::FIRST>(task_init, task_final, controller_->dt(), trajectory_duration_));
58+
trajectory_dd.push_back(trajs::min_jerk_trajectory<trajs::d_order::SECOND>(task_init, task_final, controller_->dt(), trajectory_duration_));
59+
60+
if (loop_) {
61+
trajectory.push_back(trajs::min_jerk_trajectory(task_final, task_init, controller_->dt(), trajectory_duration_));
62+
trajectory_d.push_back(trajs::min_jerk_trajectory<trajs::d_order::FIRST>(task_final, task_init, controller_->dt(), trajectory_duration_));
63+
trajectory_dd.push_back(trajs::min_jerk_trajectory<trajs::d_order::SECOND>(task_final, task_init, controller_->dt(), trajectory_duration_));
64+
}
65+
66+
trajectories_.push_back(trajectory);
67+
trajectories_d_.push_back(trajectory_d);
68+
trajectories_dd_.push_back(trajectory_dd);
69+
}
70+
}
71+
72+
void MoveFeet::update(const controllers::SensorData& sensor_data)
73+
{
74+
75+
for (uint i = 0; i < task_names_.size(); i++) {
76+
if (traj_selector_ < trajectories_[i].size()) {
77+
78+
auto ref = trajectories_[i][traj_selector_][time_];
79+
Eigen::VectorXd ref_vec(12);
80+
tsid::math::SE3ToVector(ref, ref_vec);
81+
82+
tsid::trajectories::TrajectorySample sample_ref(12, 6);
83+
sample_ref.setValue(ref_vec);
84+
sample_ref.setDerivative(trajectories_d_[i][traj_selector_][time_]);
85+
sample_ref.setSecondDerivative(trajectories_dd_[i][traj_selector_][time_]);
86+
87+
std::static_pointer_cast<inria_wbc::controllers::PosTracker>(controller_)->set_se3_ref(sample_ref, task_names_[i]);
88+
std::static_pointer_cast<inria_wbc::controllers::PosTracker>(controller_)->set_contact_se3_ref(sample_ref, contact_names_[i]);
89+
}
90+
}
91+
92+
controller_->update(sensor_data);
93+
++time_;
94+
if (trajectories_.size() > 0) {
95+
if (time_ == trajectories_[0][traj_selector_].size()) {
96+
time_ = 0;
97+
traj_selector_ = ++traj_selector_;
98+
if (loop_)
99+
traj_selector_ = traj_selector_ % trajectories_[0].size();
100+
}
101+
}
102+
}
103+
} // namespace generic
104+
} // namespace behaviors
105+
} // namespace inria_wbc

‎src/controllers/pos_tracker.cpp

+8-1
Original file line numberDiff line numberDiff line change
@@ -237,6 +237,12 @@ namespace inria_wbc {
237237
task->setReference(sample);
238238
}
239239

240+
void PosTracker::set_contact_se3_ref(tsid::trajectories::TrajectorySample& sample, const std::string& contact_name)
241+
{
242+
auto c = std::dynamic_pointer_cast<tsid::contacts::Contact6dExt>(contact(contact_name));
243+
c->setReference(sample);
244+
}
245+
240246
void PosTracker::remove_contact(const std::string& contact_name)
241247
{
242248
if (verbose_)
@@ -278,7 +284,8 @@ namespace inria_wbc {
278284

279285
auto foot_mass = robot_->model().inertias[robot_->model().getJointId(ankle_frame)].mass();
280286
// auto contact_normal = contact(contact_name)->getContactNormal();
281-
force_tsid.head(3) += foot_mass * robot_->model().gravity.linear();;
287+
force_tsid.head(3) += foot_mass * robot_->model().gravity.linear();
288+
;
282289
Eigen::Vector3d tmp = force_tsid.head(3);
283290
force_tsid.tail(3) -= (ankle_world.translation() - sole_world.translation()).cross(tmp);
284291
}

0 commit comments

Comments
 (0)