Skip to content

Commit f950d95

Browse files
jorisvjorisv
committed
scalar: Fix build with custom scalar
1 parent d15e1d2 commit f950d95

23 files changed

+314
-82
lines changed

‎bindings/python/algorithm/admm-solver.cpp‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,7 @@ namespace pinocchio
168168
;
169169
}
170170

171+
#ifdef PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE
171172
class_
172173
.def(
173174
"solve",
@@ -200,7 +201,7 @@ namespace pinocchio
200201
bp::arg("admm_update_rule") = ADMMUpdateRule::SPECTRAL,
201202
bp::arg("stat_record") = false),
202203
"Solve the constrained conic problem, starting from the optional initial guess.");
203-
#ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
204+
#ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
204205
{
205206
typedef Eigen::AccelerateLLT<context::SparseMatrix> AccelerateLLT;
206207
typedef DelassusOperatorSparseTpl<context::Scalar, context::Options, AccelerateLLT>
@@ -216,8 +217,10 @@ namespace pinocchio
216217
bp::arg("stat_record") = false),
217218
"Solve the constrained conic problem, starting from the optional initial guess.");
218219
}
219-
#endif
220+
#endif // ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
221+
#endif // ifdef PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE
220222

223+
#ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
221224
bp::def(
222225
"computeConeProjection",
223226
computeConeProjection_wrapper<ConstraintModel, ConstraintModelAllocator>,
@@ -247,6 +250,7 @@ namespace pinocchio
247250
computeDeSaxeCorrection_wrapper<ConstraintModel, ConstraintModelAllocator>,
248251
bp::args("constraint_models", "velocities"),
249252
"Compute the complementarity shift associated to the De Saxé function.");
253+
#endif // ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
250254
}
251255
//
252256
// template<typename S, int O>

‎bindings/python/algorithm/constraints/expose-cones.cpp‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,29 @@
1313
// #include "pinocchio/bindings/python/serialization/serialization.hpp"
1414
#include "pinocchio/bindings/python/utils/std-aligned-vector.hpp"
1515

16+
#ifdef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
17+
namespace eigenpy
18+
{
19+
// has_operator_equal return true for CoulombFrictionCone and DualCoulombFrictionCone with casadi
20+
// but it should not.
21+
// We provide specialization to enforce good result.
22+
template<>
23+
struct has_operator_equal<
24+
pinocchio::python::context::CoulombFrictionCone,
25+
pinocchio::python::context::CoulombFrictionCone>
26+
{
27+
typedef std::false_type type;
28+
};
29+
template<>
30+
struct has_operator_equal<
31+
pinocchio::python::context::DualCoulombFrictionCone,
32+
pinocchio::python::context::DualCoulombFrictionCone>
33+
{
34+
typedef std::false_type type;
35+
};
36+
} // namespace eigenpy
37+
#endif // ifdef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
38+
1639
namespace pinocchio
1740
{
1841
namespace python

‎bindings/python/algorithm/expose-constrained-dynamics.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -120,7 +120,7 @@ namespace pinocchio
120120

121121
ContactCholeskyDecompositionPythonVisitor<context::ContactCholeskyDecomposition>::expose();
122122

123-
exposeConstraintDynamicsFor<RigidConstraintModel>();
123+
exposeConstraintDynamicsFor<context::RigidConstraintModel>();
124124
// exposeConstraintDynamicsFor<WeldConstraintModel>();
125125
}
126126
} // namespace python

‎bindings/python/algorithm/pgs-solver.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@ namespace pinocchio
5757
void run(ConstraintModelBase<ConstraintModel> * ptr = 0)
5858
{
5959
PINOCCHIO_UNUSED_VARIABLE(ptr);
60+
#ifdef PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE
6061
class_
6162
.def(
6263
"solve", solve_wrapper<context::MatrixXs, ConstraintModel>,
@@ -72,6 +73,7 @@ namespace pinocchio
7273
bp::arg("solve_ncp") = true, bp::arg("stat_record") = false),
7374
"Solve the constrained conic problem composed of problem data (G,g,cones) and starting "
7475
"from the initial guess.");
76+
#endif // ifdef PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE
7577
}
7678

7779
// template<typename S, int O>

‎include/pinocchio/algorithm/constraints/box-set.hpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
#include "pinocchio/math/fwd.hpp"
99
#include "pinocchio/algorithm/constraints/fwd.hpp"
1010
#include "pinocchio/algorithm/constraints/set-base.hpp"
11+
#include "pinocchio/math/matrix.hpp"
1112

1213
namespace pinocchio
1314
{
@@ -60,7 +61,8 @@ namespace pinocchio
6061
, m_ub(ub)
6162
{
6263
PINOCCHIO_CHECK_INPUT_ARGUMENT(
63-
(m_lb.array() <= m_ub.array()).all(), "Some components of lb are greater than ub");
64+
arrayCompareAll(m_lb, m_ub, internal::ComparisonOperators::LE),
65+
"Some components of lb are greater than ub");
6466
}
6567

6668
/// \brief Copy constructor.
@@ -127,7 +129,7 @@ namespace pinocchio
127129
const Eigen::MatrixBase<VectorLikeIn> & x,
128130
const Eigen::MatrixBase<VectorLikeOut> & res_) const
129131
{
130-
res_.const_cast_derived() = x.array().max(m_lb.array()).min(m_ub.array());
132+
pinocchio::arrayBound(x, m_lb, m_ub, res_);
131133
}
132134

133135
/// \brief Project a vector x such that scale * res is in the box.

‎include/pinocchio/algorithm/constraints/coulomb-friction-cone.hpp‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
#include "pinocchio/algorithm/constraints/cone-base.hpp"
1010
#include "pinocchio/math/fwd.hpp"
1111
#include "pinocchio/math/comparison-operators.hpp"
12+
#include "pinocchio/utils/check.hpp"
1213

1314
namespace pinocchio
1415
{
@@ -135,12 +136,12 @@ namespace pinocchio
135136
const Vector2Plain t = x.template head<2>();
136137
const Scalar t_norm = t.norm();
137138

138-
if (mu * t_norm <= -z)
139+
if (check_expression_if_real<Scalar>(mu * t_norm <= -z))
139140
{
140141
res.setZero();
141142
return;
142143
}
143-
else if (t_norm <= mu_z)
144+
else if (check_expression_if_real<Scalar>(t_norm <= mu_z))
144145
{
145146
res = x;
146147
return;
@@ -149,7 +150,7 @@ namespace pinocchio
149150
{
150151
res.template head<2>() = (mu / t_norm) * t;
151152
res[2] = 1;
152-
res.normalize();
153+
pinocchio::normalize(res);
153154
const Scalar scale = x.dot(res);
154155
res *= scale;
155156
return;
@@ -220,7 +221,7 @@ namespace pinocchio
220221

221222
res[2] = math::max(Scalar(0), f[2]);
222223
const Scalar mu_fz = mu * res[2];
223-
if (ft_norm > mu_fz)
224+
if (check_expression_if_real<Scalar>(ft_norm > mu_fz))
224225
{
225226
res.template head<2>() = Scalar(mu_fz / ft_norm) * ft;
226227
}
@@ -352,12 +353,12 @@ namespace pinocchio
352353
const Eigen::Matrix<Scalar, 2, 1> t = x.template head<2>();
353354
const Scalar t_norm = t.norm();
354355

355-
if (t_norm <= -mu * z)
356+
if (check_expression_if_real<Scalar>(t_norm <= -mu * z))
356357
{
357358
res.setZero();
358359
return;
359360
}
360-
else if (mu * t_norm <= z)
361+
else if (check_expression_if_real<Scalar>(mu * t_norm <= z))
361362
{
362363
res = x;
363364
return;
@@ -366,7 +367,7 @@ namespace pinocchio
366367
{
367368
res.template head<2>() = t;
368369
res[2] = mu * t_norm;
369-
res.normalize();
370+
pinocchio::normalize(res);
370371
const Scalar scale = x.dot(res);
371372
res *= scale;
372373
return;

‎include/pinocchio/algorithm/constraints/joint-limit-constraint.hxx‎

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -85,16 +85,18 @@ namespace pinocchio
8585
const int q_index = idx_q + j_qi;
8686
const int q_reduce_index = nq_reduce + j_qi;
8787

88-
if (!(lb[q_index] == -std::numeric_limits<Scalar>::max()
89-
|| lb[q_index] == -std::numeric_limits<Scalar>::infinity()))
88+
if (!(check_expression_if_real<Scalar>(lb[q_index] == -std::numeric_limits<Scalar>::max())
89+
|| check_expression_if_real<Scalar>(
90+
lb[q_index] == -std::numeric_limits<Scalar>::infinity())))
9091
{
9192
activable_idx_rows_lower.push_back(idx_row);
9293
activable_idx_qs_lower.push_back(q_index);
9394
activable_idx_qs_reduce_lower.push_back(q_reduce_index);
9495
is_joint_really_active = true;
9596
}
96-
if (!(ub[q_index] == +std::numeric_limits<Scalar>::max()
97-
|| ub[q_index] == +std::numeric_limits<Scalar>::infinity()))
97+
if (!(check_expression_if_real<Scalar>(ub[q_index] == +std::numeric_limits<Scalar>::max())
98+
|| check_expression_if_real<Scalar>(
99+
ub[q_index] == +std::numeric_limits<Scalar>::infinity())))
98100
{
99101
activable_idx_rows_upper.push_back(idx_row);
100102
activable_idx_qs_upper.push_back(q_index);
@@ -264,7 +266,8 @@ namespace pinocchio
264266
const Eigen::DenseIndex ie = static_cast<Eigen::DenseIndex>(i);
265267
const Eigen::DenseIndex idx_q = activable_idx_qs[i];
266268
activable_constraint_residual[ie] = bound_position_limit[ie] - data.q_in[idx_q];
267-
if (activable_constraint_residual[ie] >= -bound_position_margin[ie])
269+
if (check_expression_if_real<Scalar>(
270+
activable_constraint_residual[ie] >= -bound_position_margin[ie]))
268271
{
269272
active_set_indexes.push_back(i);
270273
active_idx_rows.push_back(activable_idx_rows[i]);
@@ -281,7 +284,8 @@ namespace pinocchio
281284
const Eigen::DenseIndex ie = static_cast<Eigen::DenseIndex>(i);
282285
const Eigen::DenseIndex idx_q = activable_idx_qs[i];
283286
activable_constraint_residual[ie] = bound_position_limit[ie] - data.q_in[idx_q];
284-
if (activable_constraint_residual[ie] <= bound_position_margin[ie])
287+
if (check_expression_if_real<Scalar>(
288+
activable_constraint_residual[ie] <= bound_position_margin[ie]))
285289
{
286290
active_set_indexes.push_back(i);
287291
active_idx_rows.push_back(activable_idx_rows[i]);

‎include/pinocchio/algorithm/constraints/orthant-cone.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#define __pinocchio_algorithm_constraints_orthant_cone_hpp__
77

88
#include "pinocchio/math/fwd.hpp"
9+
#include "pinocchio/math/matrix.hpp"
910
#include "pinocchio/algorithm/constraints/cone-base.hpp"
1011

1112
namespace pinocchio
@@ -227,7 +228,7 @@ namespace pinocchio
227228
const Eigen::MatrixBase<VectorLikeIn> & x,
228229
const Eigen::MatrixBase<VectorLikeOut> & res_) const
229230
{
230-
res_.const_cast_derived() = x.array().min(Scalar(0)).matrix();
231+
pinocchio::arrayMin(x, Scalar(0), res_);
231232
}
232233

233234
/// \brief Project the value given as input for the given row index.

‎include/pinocchio/algorithm/constraints/point-constraint-model-base.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -420,7 +420,7 @@ namespace pinocchio
420420
- (af1.linear() + vf1.angular().cross(vf1.linear()));
421421
acceleration_error.noalias() -= af1.angular().cross(position_error);
422422
acceleration_error.noalias() += vf1.angular().cross(vf1.angular().cross(position_error));
423-
acceleration_error.noalias() -= 2 * vf1.angular().cross(velocity_error_component1);
423+
acceleration_error.noalias() -= Scalar(2.) * vf1.angular().cross(velocity_error_component1);
424424

425425
cdata.A1_world = this->getA1(cdata, WorldFrameTag());
426426
cdata.A2_world = this->getA2(cdata, WorldFrameTag());

‎include/pinocchio/algorithm/contact-inverse-dynamics.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -106,7 +106,7 @@ namespace pinocchio
106106
const Scalar contact_complementarity = cone.computeConicComplementarity(
107107
Vector3(sigma_segment + desaxce_correction), lambda_segment);
108108
const Scalar dual_feasibility =
109-
std::abs(math::min(0., sigma_segment(2))); // proxy of dual feasibility
109+
math::fabs(math::min(Scalar(0.), sigma_segment(2))); // proxy of dual feasibility
110110
settings.absolute_residual = math::max(
111111
settings.absolute_residual, math::max(contact_complementarity, dual_feasibility));
112112

0 commit comments

Comments
 (0)