Skip to content

Commit f9eb135

Browse files
edantecedantec
committed
First initialization of arm mpc
1 parent 77257a3 commit f9eb135

File tree

12 files changed

+863
-1
lines changed

12 files changed

+863
-1
lines changed

‎bindings/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,12 @@ set(
99
expose-robot-handler.cpp
1010
expose-problem.cpp
1111
expose-mpc.cpp
12+
expose-arm-mpc.cpp
1213
expose-interpolate.cpp
1314
expose-centroidal.cpp
1415
expose-fulldynamics.cpp
1516
expose-kinodynamics.cpp
17+
expose-arm-dynamics.cpp
1618
expose-inverse-dynamics.cpp
1719
expose-friction-compensation.cpp
1820
)

‎bindings/expose-arm-dynamics.cpp‎

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
#include "simple-mpc/arm-dynamics.hpp"
2+
#include "simple-mpc/python.hpp"
3+
4+
#include <eigenpy/std-map.hpp>
5+
6+
namespace simple_mpc::python
7+
{
8+
9+
auto * createArmDynamics(const bp::dict & settings, const RobotModelHandler & model_handler)
10+
{
11+
ArmDynamicsSettings conf;
12+
conf.timestep = bp::extract<double>(settings["timestep"]);
13+
conf.w_x = bp::extract<Eigen::MatrixXd>(settings["w_x"]);
14+
conf.w_u = bp::extract<Eigen::MatrixXd>(settings["w_u"]);
15+
conf.w_frame = bp::extract<Eigen::MatrixXd>(settings["w_frame"]);
16+
17+
conf.gravity = bp::extract<Eigen::Vector3d>(settings["gravity"]);
18+
19+
conf.qmin = bp::extract<Eigen::VectorXd>(settings["qmin"]);
20+
conf.qmax = bp::extract<Eigen::VectorXd>(settings["qmax"]);
21+
22+
conf.umin = bp::extract<Eigen::VectorXd>(settings["umin"]);
23+
conf.umax = bp::extract<Eigen::VectorXd>(settings["umax"]);
24+
25+
conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);
26+
conf.torque_limits = bp::extract<bool>(settings["torque_limits"]);
27+
28+
conf.ee_name = bp::extract<std::string>(settings["ee_name"]);
29+
30+
return new ArmDynamicsOCP(conf, model_handler);
31+
}
32+
33+
bp::dict getSettingsArm(ArmDynamicsOCP & self)
34+
{
35+
ArmDynamicsSettings conf = self.getSettings();
36+
bp::dict settings;
37+
settings["timestep"] = conf.timestep;
38+
settings["w_x"] = conf.w_x;
39+
settings["w_u"] = conf.w_u;
40+
settings["w_frame"] = conf.w_frame;
41+
settings["gravity"] = conf.gravity;
42+
settings["qmin"] = conf.qmin;
43+
settings["qmax"] = conf.qmax;
44+
settings["umin"] = conf.umin;
45+
settings["umax"] = conf.umax;
46+
settings["kinematics_limits"] = conf.kinematics_limits;
47+
settings["torque_limits"] = conf.torque_limits;
48+
settings["ee_name"] = conf.ee_name;
49+
50+
return settings;
51+
}
52+
53+
void exposeArmDynamicsOcp()
54+
{
55+
bp::register_ptr_to_python<shared_ptr<ArmDynamicsOCP>>();
56+
57+
bp::class_<ArmDynamicsOCP, boost::noncopyable>("ArmDynamicsOCP", bp::no_init)
58+
.def(
59+
"__init__",
60+
bp::make_constructor(&createArmDynamics, bp::default_call_policies(), ("settings"_a, "model_handler")))
61+
.def("getSettings", &getSettingsArm)
62+
.def("createStage", &ArmDynamicsOCP::createStage, bp::args("self", "reaching", "reach_pose"));
63+
}
64+
65+
} // namespace simple_mpc::python

‎bindings/expose-arm-mpc.cpp‎

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
///////////////////////////////////////////////////////////////////////////////
2+
// BSD 2-Clause License
3+
//
4+
// Copyright (C) 2024, INRIA
5+
// Copyright note valid unless otherwise stated in individual files.
6+
// All rights reserved.
7+
///////////////////////////////////////////////////////////////////////////////
8+
9+
#include <pinocchio/bindings/python/utils/pickle-map.hpp>
10+
#include <pinocchio/fwd.hpp>
11+
12+
#include <eigenpy/deprecation-policy.hpp>
13+
#include <eigenpy/eigenpy.hpp>
14+
#include <eigenpy/std-unique-ptr.hpp>
15+
#include <eigenpy/std-vector.hpp>
16+
17+
#include "simple-mpc/arm-dynamics.hpp"
18+
#include "simple-mpc/arm-mpc.hpp"
19+
#include "simple-mpc/python.hpp"
20+
21+
namespace simple_mpc
22+
{
23+
namespace python
24+
{
25+
namespace bp = boost::python;
26+
using eigenpy::StdVectorPythonVisitor;
27+
28+
ArmMPC * createArmMPC(const bp::dict & settings, std::shared_ptr<ArmDynamicsOCP> problem)
29+
{
30+
ArmMPCSettings conf;
31+
32+
conf.TOL = bp::extract<double>(settings["TOL"]);
33+
conf.mu_init = bp::extract<double>(settings["mu_init"]);
34+
conf.max_iters = bp::extract<std::size_t>(settings["max_iters"]);
35+
conf.num_threads = bp::extract<std::size_t>(settings["num_threads"]);
36+
conf.timestep = bp::extract<double>(settings["timestep"]);
37+
38+
return new ArmMPC{conf, problem};
39+
}
40+
41+
bp::dict getSettings(ArmMPC & self)
42+
{
43+
ArmMPCSettings & conf = self.settings_;
44+
bp::dict settings;
45+
settings["TOL"] = conf.TOL;
46+
settings["mu_init"] = conf.mu_init;
47+
settings["max_iters"] = conf.max_iters;
48+
settings["num_threads"] = conf.num_threads;
49+
settings["timestep"] = conf.timestep;
50+
51+
return settings;
52+
}
53+
54+
void exposeArmMPC()
55+
{
56+
using StageVec = std::vector<std::shared_ptr<StageModel>>;
57+
StdVectorPythonVisitor<StageVec, true>::expose(
58+
"StdVec_StageModel", eigenpy::details::overload_base_get_item_for_std_vector<StageVec>());
59+
60+
bp::class_<ArmMPC, boost::noncopyable>("ArmMPC", bp::no_init)
61+
.def("__init__", bp::make_constructor(&createArmMPC, bp::default_call_policies()))
62+
.def("getSettings", &getSettings)
63+
.def("generateReachHorizon", &ArmMPC::generateReachHorizon, bp::args("self", "reach_pose"))
64+
.def("iterate", &ArmMPC::iterate, bp::args("self", "x"))
65+
.def("setReferencePose", &ArmMPC::setReferencePose, bp::args("self", "t", "pose_ref"))
66+
.def("getReferencePose", &ArmMPC::getReferencePose, bp::args("self", "t"))
67+
.def_readwrite("x_reference", &ArmMPC::x_reference_)
68+
.def_readonly("ocp_handler", &ArmMPC::ocp_handler_)
69+
.def("switchToReach", &ArmMPC::switchToReach, ("self"_a, "reach_pose"))
70+
.def("switchToRest", &ArmMPC::switchToRest, "self"_a)
71+
.def("getStateDerivative", &ArmMPC::getStateDerivative, ("self"_a, "t"))
72+
.def(
73+
"getModelHandler", &ArmMPC::getModelHandler, "self"_a, bp::return_internal_reference<>(),
74+
"Get the robot model handler.")
75+
.def(
76+
"getDataHandler", &ArmMPC::getDataHandler, "self"_a, bp::return_internal_reference<>(),
77+
"Get the robot data handler.")
78+
.def(
79+
"getTrajOptProblem", &ArmMPC::getTrajOptProblem, "self"_a, bp::return_internal_reference<>(),
80+
"Get the trajectory optimal problem.")
81+
.def(
82+
"getReachHorizon", &ArmMPC::getReachHorizon, "self"_a, bp::return_internal_reference<>(),
83+
"Get the reach horizon.")
84+
.add_property("solver", bp::make_getter(&ArmMPC::solver_, eigenpy::ReturnInternalStdUniquePtr{}))
85+
.add_property("xs", &ArmMPC::xs_)
86+
.add_property("us", &ArmMPC::us_)
87+
.add_property("Ks", &ArmMPC::Ks_);
88+
}
89+
90+
} // namespace python
91+
} // namespace simple_mpc

‎bindings/module.cpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,9 @@ namespace simple_mpc::python
1414
void exposeFullDynamicsOcp();
1515
void exposeCentroidalOcp();
1616
void exposeKinodynamicsOcp();
17+
void exposeArmDynamicsOcp();
1718
void exposeMPC();
19+
void exposeArmMPC();
1820
void exposeIDSolver();
1921
void exposeIKIDSolver();
2022
void exposeInterpolator();
@@ -35,7 +37,9 @@ namespace simple_mpc::python
3537
exposeFullDynamicsOcp();
3638
exposeCentroidalOcp();
3739
exposeKinodynamicsOcp();
40+
exposeArmDynamicsOcp();
3841
exposeMPC();
42+
exposeArmMPC();
3943
exposeInterpolator();
4044
exposeInverseDynamics();
4145
exposeFrictionCompensation();

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

Lines changed: 137 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
///////////////////////////////////////////////////////////////////////////////
2+
// BSD 2-Clause License
3+
//
4+
// Copyright (C) 2025, INRIA
5+
// Copyright note valid unless otherwise stated in individual files.
6+
// All rights reserved.
7+
///////////////////////////////////////////////////////////////////////////////
8+
#pragma once
9+
10+
#include <pinocchio/algorithm/proximal.hpp>
11+
#include <pinocchio/multibody/frame.hpp>
12+
13+
#include "simple-mpc/ocp-handler.hpp"
14+
15+
namespace simple_mpc
16+
{
17+
using namespace aligator;
18+
19+
/**
20+
* @brief Build an arm dynamics problem based on the
21+
* MultibodyFreeFwdDynamics of Aligator.
22+
*
23+
* State is defined as concatenation of joint positions and
24+
* joint velocities; control is defined as joint torques.
25+
*/
26+
27+
struct ArmDynamicsSettings
28+
{
29+
public:
30+
// timestep in problem shooting nodes
31+
double timestep;
32+
33+
// Cost function weights
34+
Eigen::MatrixXd w_x; // State
35+
Eigen::MatrixXd w_u; // Control
36+
Eigen::MatrixXd w_frame; // End effector placement
37+
38+
// Physics parameters
39+
Eigen::Vector3d gravity;
40+
41+
// Constraints
42+
bool torque_limits;
43+
bool kinematics_limits;
44+
45+
// Control limits
46+
Eigen::VectorXd umin;
47+
Eigen::VectorXd umax;
48+
49+
// Kinematics limits
50+
Eigen::VectorXd qmin;
51+
Eigen::VectorXd qmax;
52+
53+
std::string ee_name;
54+
};
55+
56+
class ArmDynamicsOCP
57+
{
58+
public:
59+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60+
61+
// Constructors
62+
explicit ArmDynamicsOCP(const ArmDynamicsSettings & settings, const RobotModelHandler & model_handler);
63+
64+
SIMPLE_MPC_DEFINE_DEFAULT_MOVE_CTORS(ArmDynamicsOCP);
65+
66+
virtual ~ArmDynamicsOCP() = default;
67+
68+
// Create one TrajOptProblem
69+
void createProblem(const ConstVectorRef & x0, const size_t horizon);
70+
71+
// Create one ArmDynamics stage
72+
StageModel createStage(const bool reaching = false, const Eigen::Vector3d & reach_pose = Eigen::Vector3d::Zero());
73+
74+
// Manage terminal cost and constraint
75+
CostStack createTerminalCost();
76+
77+
// Getters and setters
78+
CostStack * getCostStack(std::size_t t);
79+
void deactivateReach(const std::size_t t);
80+
void activateReach(const std::size_t t);
81+
void setReferencePose(const std::size_t t, const Eigen::Vector3d & pose_ref);
82+
const Eigen::Vector3d getReferencePose(const std::size_t t);
83+
const Eigen::VectorXd getProblemState(const RobotDataHandler & data_handler);
84+
void setReferenceState(const std::size_t t, const ConstVectorRef & x_ref);
85+
const ConstVectorRef getReferenceState(const std::size_t t);
86+
ArmDynamicsSettings getSettings()
87+
{
88+
return settings_;
89+
}
90+
std::size_t getSize() const
91+
{
92+
return problem_->numSteps();
93+
}
94+
TrajOptProblem & getProblem()
95+
{
96+
assert(problem_);
97+
return *problem_;
98+
}
99+
100+
const TrajOptProblem & getProblem() const
101+
{
102+
assert(problem_);
103+
return *problem_;
104+
}
105+
106+
const RobotModelHandler & getModelHandler() const
107+
{
108+
return model_handler_;
109+
}
110+
int getNu()
111+
{
112+
return nv_;
113+
}
114+
115+
protected:
116+
// Problem settings
117+
ArmDynamicsSettings settings_;
118+
pinocchio::FrameIndex ee_id_;
119+
120+
// Size of the problem
121+
int nq_;
122+
int nv_;
123+
int ndx_;
124+
bool problem_initialized_ = false;
125+
bool terminal_constraint_ = false;
126+
127+
/// State reference
128+
Eigen::VectorXd x0_;
129+
130+
/// The robot model
131+
RobotModelHandler model_handler_;
132+
133+
/// The reference shooting problem storing all shooting nodes
134+
std::unique_ptr<TrajOptProblem> problem_;
135+
};
136+
137+
} // namespace simple_mpc

0 commit comments

Comments
 (0)