GCC Code Coverage Report

Directory: ./
File: python/quadruped_walkgen/quadruped_augmented.cpp
Date: 2024-12-02 00:24:10
Exec Total Coverage
Lines: 0 98 0.0%
Branches: 0 148 0.0%
Line Branch Exec Source
1
2 #include "action-base.hpp"
3 #include "core.hpp"
4
5 namespace quadruped_walkgen {
6 namespace python {
7
8 ✗ void exposeActionQuadrupedAugmented() {
9 ✗ bp::class_<ActionModelQuadrupedAugmented, bp::bases<ActionModelAbstract>>(
10 "ActionModelQuadrupedAugmented",
11 "Quadruped action model, non linear.\n\n"
12 "The model is based on simplified dynamic model\n"
13 " xnext = Ax + Bu + g,\n"
14 "The lever arms assumption is not take into account : the B matrix and "
15 "its derivative depends on the state x\n"
16 "where x i the state vector defined as : \n"
17 "x = [x , y, z, rool, pitch, yaw, Vx, Vy, Vz, Vrool, Vpitch, Vyaw] , 12x "
18 "\n\n"
19 "and u is the groud reaction forces at each 4 foot, defined as : \n"
20 "u = [fx1 , fy1, fz1, ... fz4], 12x",
21 ✗ bp::init<Eigen::Matrix<double, 3, 1>>(
22 ✗ bp::args("self", "offset_CoM"),
23 "Initialize the quadruped action model."))
24 ✗ .def("calc", &ActionModelQuadrupedAugmented::calc,
25 ✗ bp::args("self", "data", "x", "u"),
26 "Compute the next state and cost value.\n\n"
27 "It describes the time-discrete evolution of the quadruped system.\n"
28 "Additionally it computes the cost value associated to this "
29 "discrete\n"
30 "state and control pair.\n"
31 ":param data: action data\n"
32 ":param x: time-discrete state vector\n"
33 ":param u: time-discrete control input")
34 .def<void (ActionModelQuadrupedAugmented::*)(
35 const boost::shared_ptr<ActionDataAbstract>&,
36 ✗ const Eigen::Ref<const Eigen::VectorXd>&)>(
37 ✗ "calc", &ActionModelAbstract::calc, bp::args("self", "data", "x"))
38 ✗ .def("calcDiff", &ActionModelQuadrupedAugmented::calcDiff,
39 ✗ bp::args("self", "data", "x", "u"),
40 "Compute the derivatives of the quadruped dynamics and cost "
41 "functions.\n\n"
42 "It computes the partial derivatives of the quadruped system and "
43 "the\n"
44 "cost function. It assumes that calc has been run first.\n"
45 "This function builds a quadratic approximation of the\n"
46 "action model (i.e. dynamical system and cost function).\n"
47 ":param data: action data\n"
48 ":param x: time-discrete state vector\n"
49 ":param u: time-discrete control input\n")
50 .def<void (ActionModelQuadrupedAugmented::*)(
51 const boost::shared_ptr<ActionDataAbstract>&,
52 ✗ const Eigen::Ref<const Eigen::VectorXd>&)>(
53 "calcDiff", &ActionModelAbstract::calcDiff,
54 ✗ bp::args("self", "data", "x"))
55 ✗ .def("createData", &ActionModelQuadrupedAugmented::createData,
56 ✗ bp::args("self"), "Create the quadruped action data.")
57 ✗ .def("updateModel", &ActionModelQuadrupedAugmented::update_model,
58 ✗ bp::args("self", "l_feet", "l_stop", "xref", "S"),
59 "Update the quadruped model depending on the position of the foot "
60 "in the local frame\n\n"
61 ":param l_feet : 3x4, Matrix representing the position of the foot "
62 "in the local frame \n "
63 " Each colum represents the position of one foot : "
64 "x,y,z"
65 ":param xref : 12x1, Vector representing the reference state."
66 ":param S : 4x1, Vector representing the foot in contact with the "
67 "ground."
68 " S = [1 0 0 1] --> Foot 1 and 4 in contact.")
69 ✗ .add_property(
70 "forceWeights",
71 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_force_weights,
72 ✗ bp::return_internal_reference<>()),
73 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_force_weights),
74 "Weights on the control input : ground reaction forces")
75 ✗ .add_property(
76 "stateWeights",
77 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_state_weights,
78 ✗ bp::return_internal_reference<>()),
79 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_state_weights),
80 "Weights on the state vector")
81 ✗ .add_property("heuristicWeights",
82 ✗ bp::make_function(
83 &ActionModelQuadrupedAugmented::get_heuristic_weights,
84 ✗ bp::return_internal_reference<>()),
85 ✗ bp::make_function(
86 &ActionModelQuadrupedAugmented::set_heuristic_weights),
87 "Weights on the heuristic term")
88 ✗ .add_property(
89 "stopWeights",
90 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_stop_weights,
91 ✗ bp::return_internal_reference<>()),
92 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_stop_weights),
93 "Weights on the last position term")
94 ✗ .add_property(
95 "frictionWeights",
96 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_friction_weight,
97 ✗ bp::return_value_policy<bp::return_by_value>()),
98 ✗ bp::make_function(
99 &ActionModelQuadrupedAugmented::set_friction_weight),
100 "Weight on friction cone term")
101 ✗ .add_property(
102 "mu",
103 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_mu,
104 ✗ bp::return_value_policy<bp::return_by_value>()),
105 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_mu),
106 "Friction coefficient")
107 ✗ .add_property(
108 "mass",
109 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_mass,
110 ✗ bp::return_value_policy<bp::return_by_value>()),
111 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_mass),
112 "Mass \n Warning : The model needs to be updated")
113 ✗ .add_property(
114 "dt",
115 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_dt,
116 ✗ bp::return_value_policy<bp::return_by_value>()),
117 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_dt),
118 "Minimum normal force allowed for feet in contact with the ground \n "
119 "Warning : The model needs to be "
120 "updated")
121 ✗ .add_property(
122 "min_fz",
123 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_min_fz_contact,
124 ✗ bp::return_value_policy<bp::return_by_value>()),
125 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_min_fz_contact),
126 "dt \n Warning : The model needs to be updated")
127 ✗ .add_property(
128 "max_fz",
129 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_max_fz_contact,
130 ✗ bp::return_value_policy<bp::return_by_value>()),
131 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_max_fz_contact),
132 "dt \n Warning : The model needs to be updated")
133 ✗ .add_property(
134 "gI",
135 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_gI,
136 ✗ bp::return_value_policy<bp::return_by_value>()),
137 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_gI),
138 "Inertia matrix of the robot in body frame (found in urdf) \n "
139 "Warning : The model needs to be updated")
140 ✗ .add_property("A",
141 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_A,
142 ✗ bp::return_internal_reference<>()),
143 "get A matrix")
144 ✗ .add_property("B",
145 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_B,
146 ✗ bp::return_internal_reference<>()),
147 "get B matrix")
148 ✗ .add_property(
149 "shoulder_hlim",
150 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_shoulder_hlim,
151 ✗ bp::return_value_policy<bp::return_by_value>()),
152 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_shoulder_hlim),
153 "Shoulder height limit ")
154 ✗ .add_property(
155 "shoulderContactWeight",
156 ✗ bp::make_function(
157 &ActionModelQuadrupedAugmented::get_shoulder_contact_weight,
158 ✗ bp::return_value_policy<bp::return_by_value>()),
159 ✗ bp::make_function(
160 &ActionModelQuadrupedAugmented::set_shoulder_contact_weight),
161 "shoulder Weights terms (array of size 8) ")
162 ✗ .add_property(
163 "symmetry_term",
164 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_symmetry_term,
165 ✗ bp::return_value_policy<bp::return_by_value>()),
166 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_symmetry_term),
167 "symmetry term for the foot position heuristic")
168 ✗ .add_property(
169 "relative_forces",
170 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_relative_forces,
171 ✗ bp::return_value_policy<bp::return_by_value>()),
172 ✗ bp::make_function(
173 &ActionModelQuadrupedAugmented::set_relative_forces),
174 "activate relative forces ||fz-mg/nb_contact||^2")
175 ✗ .add_property("centrifugal_term",
176 ✗ bp::make_function(
177 &ActionModelQuadrupedAugmented::get_centrifugal_term,
178 ✗ bp::return_value_policy<bp::return_by_value>()),
179 ✗ bp::make_function(
180 &ActionModelQuadrupedAugmented::set_centrifugal_term),
181 "centrifugal term for the foot position heuristic")
182 ✗ .add_property(
183 "T_gait",
184 ✗ bp::make_function(&ActionModelQuadrupedAugmented::get_T_gait,
185 ✗ bp::return_value_policy<bp::return_by_value>()),
186 ✗ bp::make_function(&ActionModelQuadrupedAugmented::set_T_gait),
187 "Gait period, used to compute the symmetry term")
188 ✗ .add_property(
189 "shoulderReferencePosition",
190 ✗ bp::make_function(
191 &ActionModelQuadrupedAugmented::get_shoulder_reference_position,
192 ✗ bp::return_value_policy<bp::return_by_value>()),
193 ✗ bp::make_function(
194 &ActionModelQuadrupedAugmented::set_shoulder_reference_position),
195 "Bool, reference trajectory of the shoulder to compute the "
196 "shoulder-to-contact cost");
197
198 ✗ bp::register_ptr_to_python<boost::shared_ptr<ActionDataQuadrupedAugmented>>();
199
200 ✗ bp::class_<ActionDataQuadrupedAugmented, bp::bases<ActionDataAbstract>>(
201 "ActionDataQuadrupedAugmented",
202 "Action data for the non linear quadruped system.\n\n"
203 "The quadruped data, apart of common one, contains the cost residuals "
204 "used\n"
205 "for the computation of calc and calcDiff.",
206 ✗ bp::init<ActionModelQuadrupedAugmented*>(
207 ✗ bp::args("self", "model"),
208 "Create quadruped data.\n\n"
209 ":param model: quadruped action model"));
210 }
211
212 } // namespace python
213 } // namespace quadruped_walkgen
214