10#ifndef CROCODDYL_MULTIBODY_FRAMES_DEPRECATED_HPP_
11#define CROCODDYL_MULTIBODY_FRAMES_DEPRECATED_HPP_
13#include <pinocchio/multibody/fwd.hpp>
14#include <pinocchio/spatial/se3.hpp>
15#include <pinocchio/spatial/motion.hpp>
16#include <pinocchio/spatial/force.hpp>
18#include "crocoddyl/multibody/fwd.hpp"
19#include "crocoddyl/core/mathbase.hpp"
20#include "crocoddyl/multibody/friction-cone.hpp"
21#include "crocoddyl/multibody/wrench-cone.hpp"
22#include "crocoddyl/core/utils/deprecate.hpp"
26typedef std::size_t FrameIndex;
28template <
typename _Scalar>
30 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 typedef _Scalar Scalar;
33 typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
37 DEPRECATED(
"Do not use FrameTranslation",
FrameTranslationTpl(
const FrameIndex&
id,
const Vector3s& translation));
40 os <<
" id: " << X.id << std::endl
41 <<
"translation: " << std::endl
42 << X.translation.transpose() << std::endl;
49 translation = other.translation;
54 template <
typename OtherScalar>
56 return id == other.id && translation == other.translation;
63template <
typename _Scalar>
65 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 typedef _Scalar Scalar;
68 typedef typename MathBaseTpl<Scalar>::Matrix3s Matrix3s;
72 DEPRECATED(
"Do not use FrameRotation",
FrameRotationTpl(
const FrameIndex&
id,
const Matrix3s& rotation));
75 os <<
" id: " << X.id << std::endl <<
"rotation: " << std::endl << X.rotation << std::endl;
82 rotation = other.rotation;
87 template <
typename OtherScalar>
89 return id == other.id && rotation == other.rotation;
96template <
typename _Scalar>
98 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
100 typedef _Scalar Scalar;
101 typedef pinocchio::SE3Tpl<Scalar> SE3;
105 DEPRECATED(
"Do not use FramePlacement",
FramePlacementTpl(
const FrameIndex&
id,
const SE3& placement));
108 if (
this != &other) {
110 placement = other.placement;
115 template <
typename OtherScalar>
117 return id == other.id && placement == other.placement;
121 os <<
" id: " << X.id << std::endl <<
"placement: " << std::endl << X.placement << std::endl;
126 pinocchio::SE3Tpl<Scalar> placement;
129template <
typename _Scalar>
131 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133 typedef _Scalar Scalar;
134 typedef pinocchio::MotionTpl<Scalar> Motion;
138 DEPRECATED(
"Do not use FrameMotion",
FrameMotionTpl(
const FrameIndex&
id,
const Motion& motion,
139 pinocchio::ReferenceFrame reference = pinocchio::LOCAL));
142 os <<
" id: " << X.id << std::endl;
143 os <<
" motion: " << std::endl << X.motion;
144 switch (X.reference) {
145 case pinocchio::WORLD:
146 os <<
"reference: WORLD" << std::endl;
148 case pinocchio::LOCAL:
149 os <<
"reference: LOCAL" << std::endl;
151 case pinocchio::LOCAL_WORLD_ALIGNED:
152 os <<
"reference: LOCAL_WORLD_ALIGNED" << std::endl;
159 if (
this != &other) {
161 motion = other.motion;
162 reference = other.reference;
167 template <
typename OtherScalar>
169 return id == other.id && motion == other.motion && reference == other.reference;
173 pinocchio::MotionTpl<Scalar> motion;
174 pinocchio::ReferenceFrame reference;
177template <
typename _Scalar>
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
181 typedef _Scalar Scalar;
182 typedef pinocchio::ForceTpl<Scalar> Force;
184 DEPRECATED(
"Do not use FrameForce",
explicit FrameForceTpl());
186 DEPRECATED(
"Do not use FrameForce",
FrameForceTpl(
const FrameIndex&
id,
const Force& force));
189 os <<
" id: " << X.id << std::endl <<
"force: " << std::endl << X.force << std::endl;
194 if (
this != &other) {
201 template <
typename OtherScalar>
203 return id == other.id && force == other.force;
207 pinocchio::ForceTpl<Scalar> force;
210template <
typename _Scalar>
212 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
214 typedef _Scalar Scalar;
222 os <<
" id: " << X.id << std::endl <<
"cone: " << std::endl << X.cone << std::endl;
227 if (
this != &other) {
238template <
typename _Scalar>
240 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
242 typedef _Scalar Scalar;
250 os <<
"frame: " << X.id << std::endl <<
" cone: " << std::endl << X.cone << std::endl;
255 if (
this != &other) {
266template <
typename _Scalar>
268 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
270 typedef _Scalar Scalar;
271 typedef typename MathBaseTpl<Scalar>::Vector2s Vector2s;
272 typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
273 typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
278 DEPRECATED(
"Do not use FrameCoPSupport",
FrameCoPSupportTpl(
const FrameIndex&
id,
const Vector2s& box));
281 os <<
" id: " << X.get_id() << std::endl <<
"box: " << std::endl << X.get_box() << std::endl;
286 if (
this != &other) {
287 id_ = other.get_id();
288 box_ = other.get_box();
297 A_ << Scalar(0), Scalar(0), box_[0] / Scalar(2), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
298 box_[0] / Scalar(2), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(1),
299 Scalar(0), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(-1), Scalar(0), Scalar(0);
302 void set_id(FrameIndex
id) { id_ = id; }
303 void set_box(
const Vector2s& box) {
308 const FrameIndex& get_id()
const {
return id_; }
309 const Vector2s& get_box()
const {
return box_; }
310 const Matrix46& get_A()
const {
return A_; }
318template <
typename Scalar>
320 std::cerr <<
"Deprecated: Do not use FrameTranslation." << std::endl;
322template <
typename Scalar>
323FrameTranslationTpl<Scalar>::FrameTranslationTpl(
const FrameTranslationTpl<Scalar>& other)
324 : id(other.id), translation(other.translation) {
325 std::cerr <<
"Deprecated: Do not use FrameTranslation." << std::endl;
327template <
typename Scalar>
328FrameTranslationTpl<Scalar>::FrameTranslationTpl(
const FrameIndex&
id,
const Vector3s& translation)
329 : id(id), translation(translation) {
330 std::cerr <<
"Deprecated: Do not use FrameTranslation." << std::endl;
333template <
typename Scalar>
334FrameRotationTpl<Scalar>::FrameRotationTpl() : id(0), rotation(Matrix3s::Identity()) {
335 std::cerr <<
"Deprecated: Do not use FrameRotation." << std::endl;
337template <
typename Scalar>
338FrameRotationTpl<Scalar>::FrameRotationTpl(
const FrameRotationTpl<Scalar>& other)
339 : id(other.id), rotation(other.rotation) {
340 std::cerr <<
"Deprecated: Do not use FrameRotation." << std::endl;
342template <
typename Scalar>
343FrameRotationTpl<Scalar>::FrameRotationTpl(
const FrameIndex&
id,
const Matrix3s& rotation)
344 : id(id), rotation(rotation) {
345 std::cerr <<
"Deprecated: Do not use FrameRotation." << std::endl;
348template <
typename Scalar>
349FramePlacementTpl<Scalar>::FramePlacementTpl() : id(0), placement(SE3::Identity()) {
350 std::cerr <<
"Deprecated: Do not use FramePlacement." << std::endl;
352template <
typename Scalar>
353FramePlacementTpl<Scalar>::FramePlacementTpl(
const FramePlacementTpl<Scalar>& other)
354 : id(other.id), placement(other.placement) {
355 std::cerr <<
"Deprecated: Do not use FramePlacement." << std::endl;
357template <
typename Scalar>
358FramePlacementTpl<Scalar>::FramePlacementTpl(
const FrameIndex&
id,
const SE3& placement)
359 : id(id), placement(placement) {
360 std::cerr <<
"Deprecated: Do not use FramePlacement." << std::endl;
363template <
typename Scalar>
364FrameMotionTpl<Scalar>::FrameMotionTpl() : id(0), motion(Motion::Zero()), reference(pinocchio::LOCAL) {
365 std::cerr <<
"Deprecated: Do not use FrameMotion." << std::endl;
367template <
typename Scalar>
368FrameMotionTpl<Scalar>::FrameMotionTpl(
const FrameMotionTpl<Scalar>& other)
369 : id(other.id), motion(other.motion), reference(other.reference) {
370 std::cerr <<
"Deprecated: Do not use FrameMotion." << std::endl;
372template <
typename Scalar>
373FrameMotionTpl<Scalar>::FrameMotionTpl(
const FrameIndex&
id,
const Motion& motion, pinocchio::ReferenceFrame reference)
374 : id(id), motion(motion), reference(reference) {
375 std::cerr <<
"Deprecated: Do not use FrameMotion." << std::endl;
378template <
typename Scalar>
379FrameForceTpl<Scalar>::FrameForceTpl() : id(0), force(Force::Zero()) {
380 std::cerr <<
"Deprecated: Do not use FrameForce." << std::endl;
382template <
typename Scalar>
383FrameForceTpl<Scalar>::FrameForceTpl(
const FrameForceTpl<Scalar>& other) : id(other.id), force(other.force) {
384 std::cerr <<
"Deprecated: Do not use FrameForce." << std::endl;
386template <
typename Scalar>
387FrameForceTpl<Scalar>::FrameForceTpl(
const FrameIndex&
id,
const Force& force) : id(id), force(force) {
388 std::cerr <<
"Deprecated: Do not use FrameForce." << std::endl;
391template <
typename Scalar>
392FrameFrictionConeTpl<Scalar>::FrameFrictionConeTpl() : id(0), cone(FrictionCone()) {
393 std::cerr <<
"Deprecated: Do not use FrameFrictionCone." << std::endl;
395template <
typename Scalar>
396FrameFrictionConeTpl<Scalar>::FrameFrictionConeTpl(
const FrameFrictionConeTpl<Scalar>& other)
397 : id(other.id), cone(other.cone) {
398 std::cerr <<
"Deprecated: Do not use FrameFrictionCone." << std::endl;
400template <
typename Scalar>
401FrameFrictionConeTpl<Scalar>::FrameFrictionConeTpl(
const FrameIndex&
id,
const FrictionCone& cone)
402 : id(id), cone(cone) {
403 std::cerr <<
"Deprecated: Do not use FrameFrictionCone." << std::endl;
406template <
typename Scalar>
407FrameWrenchConeTpl<Scalar>::FrameWrenchConeTpl() : id(0), cone(WrenchCone()) {
408 std::cerr <<
"Deprecated: Do not use FrameWrenchCone." << std::endl;
410template <
typename Scalar>
411FrameWrenchConeTpl<Scalar>::FrameWrenchConeTpl(
const FrameWrenchConeTpl<Scalar>& other)
412 : id(other.id), cone(other.cone) {
413 std::cerr <<
"Deprecated: Do not use FrameWrenchCone." << std::endl;
415template <
typename Scalar>
416FrameWrenchConeTpl<Scalar>::FrameWrenchConeTpl(
const FrameIndex&
id,
const WrenchCone& cone) : id(id), cone(cone) {
417 std::cerr <<
"Deprecated: Do not use FrameWrenchCone." << std::endl;
420template <
typename Scalar>
421FrameCoPSupportTpl<Scalar>::FrameCoPSupportTpl() : id_(0), box_(Vector2s::Zero()) {
423 std::cerr <<
"Deprecated: Do not use FrameCoPSupport." << std::endl;
425template <
typename Scalar>
426FrameCoPSupportTpl<Scalar>::FrameCoPSupportTpl(
const FrameCoPSupportTpl<Scalar>& other)
427 : id_(other.get_id()), box_(other.get_box()), A_(other.get_A()) {
428 std::cerr <<
"Deprecated: Do not use FrameCoPSupport." << std::endl;
430template <
typename Scalar>
431FrameCoPSupportTpl<Scalar>::FrameCoPSupportTpl(
const FrameIndex&
id,
const Vector2s& box) : id_(id), box_(box) {
433 std::cerr <<
"Deprecated: Do not use FrameCoPSupport." << std::endl;
This class encapsulates a friction cone.
This class encapsulates a wrench cone.