crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frames.hpp
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_MULTIBODY_FRAMES_HPP_
11 #define CROCODDYL_MULTIBODY_FRAMES_HPP_
12 
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/multibody/friction-cone.hpp"
15 #include "crocoddyl/core/mathbase.hpp"
16 
17 #include <pinocchio/spatial/se3.hpp>
18 #include <pinocchio/spatial/motion.hpp>
19 #include <pinocchio/spatial/force.hpp>
20 
21 namespace crocoddyl {
22 
23 typedef std::size_t FrameIndex;
24 
25 template <typename _Scalar>
27  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 
29  typedef _Scalar Scalar;
30  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
31 
32  explicit FrameTranslationTpl() : frame(0), oxf(Vector3s::Zero()) {}
33  FrameTranslationTpl(const FrameTranslationTpl<Scalar>& value) : frame(value.frame), oxf(value.oxf) {}
34  FrameTranslationTpl(const FrameIndex& frame, const Vector3s& oxf) : frame(frame), oxf(oxf) {}
35  friend std::ostream& operator<<(std::ostream& os, const FrameTranslationTpl<Scalar>& X) {
36  os << " frame: " << X.frame << std::endl << "translation: " << std::endl << X.oxf.transpose() << std::endl;
37  return os;
38  }
39 
40  FrameIndex frame;
41  Vector3s oxf;
42 };
43 
44 template <typename _Scalar>
46  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47 
48  typedef _Scalar Scalar;
49  typedef typename MathBaseTpl<Scalar>::Matrix3s Matrix3s;
50 
51  explicit FrameRotationTpl() : frame(0), oRf(Matrix3s::Identity()) {}
52  FrameRotationTpl(const FrameRotationTpl<Scalar>& value) : frame(value.frame), oRf(value.oRf) {}
53  FrameRotationTpl(const FrameIndex& frame, const Matrix3s& oRf) : frame(frame), oRf(oRf) {}
54  friend std::ostream& operator<<(std::ostream& os, const FrameRotationTpl<Scalar>& X) {
55  os << " frame: " << X.frame << std::endl << "rotation: " << std::endl << X.oRf << std::endl;
56  return os;
57  }
58 
59  FrameIndex frame;
60  Matrix3s oRf;
61 };
62 
63 template <typename _Scalar>
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 
67  typedef _Scalar Scalar;
68  typedef pinocchio::SE3Tpl<Scalar> SE3;
69 
70  explicit FramePlacementTpl() : frame(0), oMf(SE3::Identity()) {}
71  FramePlacementTpl(const FramePlacementTpl<Scalar>& value) : frame(value.frame), oMf(value.oMf) {}
72  FramePlacementTpl(const FrameIndex& frame, const SE3& oMf) : frame(frame), oMf(oMf) {}
73  friend std::ostream& operator<<(std::ostream& os, const FramePlacementTpl<Scalar>& X) {
74  os << " frame: " << X.frame << std::endl << "placement: " << std::endl << X.oMf << std::endl;
75  return os;
76  }
77 
78  FrameIndex frame;
79  pinocchio::SE3Tpl<Scalar> oMf;
80 };
81 
82 template <typename _Scalar>
84  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85 
86  typedef _Scalar Scalar;
87  typedef pinocchio::MotionTpl<Scalar> Motion;
88 
89  explicit FrameMotionTpl() : frame(0), oMf(Motion::Zero()) {}
90  FrameMotionTpl(const FrameMotionTpl<Scalar>& value) : frame(value.frame), oMf(value.oMf) {}
91  FrameMotionTpl(const FrameIndex& frame, const Motion& oMf) : frame(frame), oMf(oMf) {}
92  friend std::ostream& operator<<(std::ostream& os, const FrameMotionTpl<Scalar>& X) {
93  os << " frame: " << X.frame << std::endl << "motion: " << std::endl << X.oMf << std::endl;
94  return os;
95  }
96 
97  FrameIndex frame;
98  pinocchio::MotionTpl<Scalar> oMf;
99 };
100 
101 template <typename _Scalar>
103  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
104 
105  typedef _Scalar Scalar;
106  typedef pinocchio::ForceTpl<Scalar> Force;
107 
108  explicit FrameForceTpl() : frame(0), oFf(Force::Zero()) {}
109  FrameForceTpl(const FrameForceTpl<Scalar>& value) : frame(value.frame), oFf(value.oFf) {}
110  FrameForceTpl(const FrameIndex& frame, const Force& oFf) : frame(frame), oFf(oFf) {}
111  friend std::ostream& operator<<(std::ostream& os, const FrameForceTpl<Scalar>& X) {
112  os << "frame: " << X.frame << std::endl << "force: " << std::endl << X.oFf << std::endl;
113  return os;
114  }
115 
116  FrameIndex frame;
117  pinocchio::ForceTpl<Scalar> oFf;
118 };
119 
120 template <typename _Scalar>
122  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
123 
124  typedef _Scalar Scalar;
126 
127  explicit FrameFrictionConeTpl() : frame(0), oRf(FrictionCone()) {}
128  FrameFrictionConeTpl(const FrameFrictionConeTpl<Scalar>& value) : frame(value.frame), oRf(value.oRf) {}
129  FrameFrictionConeTpl(const FrameIndex& frame, const FrictionCone& oRf) : frame(frame), oRf(oRf) {}
130  friend std::ostream& operator<<(std::ostream& os, const FrameFrictionConeTpl& X) {
131  os << "frame: " << X.frame << std::endl << " cone: " << std::endl << X.oRf << std::endl;
132  return os;
133  }
134 
135  FrameIndex frame;
136  FrictionCone oRf;
137 };
138 
139 } // namespace crocoddyl
140 
141 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_