Skip to content

Commit 3be8eb2

Browse files
author
earlaud
committed
Copy paste TSID implementation from go2_mpc-experiments
1 parent a74aa8e commit 3be8eb2

File tree

1 file changed

+154
-0
lines changed

1 file changed

+154
-0
lines changed

‎include/simple-mpc/inverse-dynamics.hpp‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
#pragma once
2+
3+
#include <simple-mpc/robot-handler.hpp>
4+
#include <tsid/contacts/contact-point.hpp>
5+
#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
6+
#include <tsid/solvers/solver-HQP-factory.hxx>
7+
#include <tsid/solvers/utils.hpp>
8+
#include <tsid/tasks/task-joint-bounds.hpp>
9+
#include <tsid/tasks/task-joint-posture.hpp>
10+
#include <tsid/tasks/task-se3-equality.hpp>
11+
#include <tsid/trajectories/trajectory-euclidian.hpp>
12+
13+
namespace simple_mpc
14+
{
15+
16+
class KinodynamicsID
17+
{
18+
public:
19+
KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler)
20+
: model_handler_(model_handler)
21+
, data_handler_(model_handler_)
22+
, robot_(model_handler_.getModel())
23+
, formulation_("tsid", robot_)
24+
{
25+
const pinocchio::Model & model = model_handler.getModel();
26+
const size_t nq = model.nq;
27+
const size_t nv = model.nv;
28+
const size_t nu = nv - 6;
29+
30+
formulation_.computeProblemData(0, model_handler.getReferenceState().head(nq), Eigen::VectorXd::Zero(nv));
31+
32+
const double kp_contact = 0.0;
33+
const double kp_posture = 1.0;
34+
const double kp_base = 0.0;
35+
36+
const double w_base = 0.;
37+
const double w_posture = 1e2;
38+
const double w_forceRef_ = 0.;
39+
const double w_motionRef_ = 0.;
40+
41+
// Prepare contact point task
42+
// const Eigen::Vector3d normal {0, 0, 1};
43+
// for (std::string foot_name : model_handler_.getFeetNames()) {
44+
// std::shared_ptr<tsid::contacts::ContactPoint> cp =
45+
// std::make_shared<tsid::contacts::ContactPoint>(foot_name, robot_, foot_name, normal, 0.3, 1.0, 100.);
46+
// cp->Kp(kp_contact * Eigen::VectorXd::Ones(3));
47+
// cp->Kd(2.0 * cp->Kp().cwiseSqrt());
48+
// cp->setReference(data_handler_.getFootPose(foot_name));
49+
// cp->useLocalFrame(false);
50+
// formulation_.addRigidContact(*cp, w_forceRef_, w_motionRef_, 0);
51+
// contactPoints_.push_back(cp);
52+
// }
53+
54+
// Add the posture task
55+
postureTask_ = std::make_shared<tsid::tasks::TaskJointPosture>("task-posture", robot_);
56+
postureTask_->Kp(kp_posture * Eigen::VectorXd::Ones(nu));
57+
postureTask_->Kd(2.0 * postureTask_->Kp().cwiseSqrt());
58+
formulation_.addMotionTask(*postureTask_, w_posture, 1);
59+
60+
samplePosture_ = tsid::trajectories::TrajectorySample(robot_.nq_actuated(), robot_.na());
61+
62+
// Add the base task
63+
baseTask_ = std::make_shared<tsid::tasks::TaskSE3Equality>("task-base", robot_, "root_joint");
64+
baseTask_->Kp(kp_base * Eigen::VectorXd::Ones(6));
65+
baseTask_->Kd(2.0 * baseTask_->Kp().cwiseSqrt());
66+
baseTask_->setReference(pose_base_);
67+
// formulation_.addMotionTask(*baseTask_, w_base, 0);
68+
69+
sampleBase_ = tsid::trajectories::TrajectorySample(12, 6);
70+
71+
// Create an HQP solver
72+
solver_ = tsid::solvers::SolverHQPFactory::createNewSolver(tsid::solvers::SOLVER_HQP_PROXQP, "solver-proxqp");
73+
solver_->resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn());
74+
75+
HQPData_ = formulation_.computeProblemData(
76+
0, model_handler.getReferenceState().head(nq), model_handler.getReferenceState().tail(nv));
77+
sol_ = solver_->solve(HQPData_);
78+
}
79+
80+
void setTarget(
81+
const Eigen::VectorXd & q_target,
82+
const Eigen::VectorXd & v_target,
83+
const Eigen::VectorXd & a_target,
84+
const std::vector<bool> & contact_state,
85+
const Eigen::VectorXd & f_target)
86+
{
87+
samplePosture_.setValue(q_target.tail(robot_.nq_actuated()));
88+
samplePosture_.setDerivative(v_target.tail(robot_.na()));
89+
samplePosture_.setSecondDerivative(a_target.tail(robot_.na()));
90+
postureTask_->setReference(samplePosture_);
91+
92+
pose_base_.rotation() = pinocchio::SE3::Quaternion(q_target[3], q_target[4], q_target[5], q_target[6]).matrix();
93+
pose_base_.translation() = q_target.head(3);
94+
tsid::math::SE3ToVector(pose_base_, sampleBase_.pos);
95+
sampleBase_.setDerivative(v_target.head(6));
96+
sampleBase_.setSecondDerivative(a_target.head(6));
97+
baseTask_->setReference(sampleBase_);
98+
99+
// for (std::size_t i = 0; i < model_handler_.getFeetNames().size(); i ++) {
100+
// std::string name = model_handler_.getFeetNames()[i];
101+
// if (contact_state[i]) {
102+
// if(!check_contact(name)) {
103+
// // formulation_.addRigidContact(*contactPoints_[i], w_forceRef_, w_motionRef_, 0);
104+
// }
105+
// else {
106+
// contactPoints_[i]->setForceReference(forces.segment(i * 3, 3));
107+
// contactPoints_[i]->setReference(robot_->framePosition(data, robot_->model().getFrameId(name)));
108+
// }
109+
// }
110+
// else {
111+
// if (check_contact(name)) {
112+
// // formulation_.removeRigidContact(name, 0);
113+
// }
114+
// }
115+
// }
116+
}
117+
118+
bool check_contact(std::string & name)
119+
{
120+
for (auto & it : formulation_.m_contacts)
121+
{
122+
if (it->contact.name() == name)
123+
{
124+
return true;
125+
}
126+
}
127+
return false;
128+
}
129+
130+
void
131+
solve(const double t, const Eigen::VectorXd & q_meas, const Eigen::VectorXd & v_meas, Eigen::VectorXd & tau_res)
132+
{
133+
HQPData_ = formulation_.computeProblemData(t, q_meas, v_meas);
134+
sol_ = solver_->solve(HQPData_);
135+
tau_res = formulation_.getActuatorForces(sol_);
136+
}
137+
138+
const simple_mpc::RobotModelHandler & model_handler_;
139+
simple_mpc::RobotDataHandler data_handler_;
140+
141+
tsid::robots::RobotWrapper robot_;
142+
tsid::InverseDynamicsFormulationAccForce formulation_;
143+
std::vector<std::shared_ptr<tsid::contacts::ContactPoint>> contactPoints_;
144+
std::shared_ptr<tsid::tasks::TaskJointPosture> postureTask_;
145+
std::shared_ptr<tsid::tasks::TaskSE3Equality> baseTask_;
146+
tsid::solvers::SolverHQPBase * solver_;
147+
tsid::solvers::HQPData HQPData_;
148+
tsid::solvers::HQPOutput sol_;
149+
tsid::trajectories::TrajectorySample samplePosture_; // TODO: no need to store it
150+
tsid::trajectories::TrajectorySample sampleBase_; // TODO: no need to store it
151+
pinocchio::SE3 pose_base_;
152+
};
153+
154+
} // namespace simple_mpc

0 commit comments

Comments
 (0)