pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
motion.hpp
1//
2// Copyright (c) 2020 INRIA
3//
4
5#ifndef __pinocchio_algorithm_utils_motion_hpp__
6#define __pinocchio_algorithm_utils_motion_hpp__
7
8#include "pinocchio/spatial/se3.hpp"
9#include "pinocchio/spatial/motion.hpp"
10#include "pinocchio/multibody/fwd.hpp"
11
12namespace pinocchio
13{
14
19 template<typename Scalar, int Options, typename MotionIn, typename MotionOut>
21 const SE3Tpl<Scalar, Options> & placement,
26 {
27 if (rf_in == rf_out)
28 {
29 m_out = m_in;
30 return;
31 }
32
33 // case: LOCAL/WORLD and WORLD/LOCAL
34 if (rf_in == LOCAL && rf_out == WORLD)
35 {
36 m_out = placement.act(m_in);
37 return;
38 }
39 if (rf_in == WORLD && rf_out == LOCAL)
40 {
41 m_out = placement.actInv(m_in);
42 return;
43 }
44
45 // case: LOCAL/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/LOCAL
47 {
48 m_out.linear().noalias() = placement.rotation() * m_in.linear();
49 m_out.angular().noalias() = placement.rotation() * m_in.angular();
50 return;
51 }
53 {
54 m_out.linear().noalias() = placement.rotation().transpose() * m_in.linear();
55 m_out.angular().noalias() = placement.rotation().transpose() * m_in.angular();
56 return;
57 }
58
59 // case: WORLD/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/WORLD
61 {
62 m_out.angular() = m_in.angular();
63 m_out.linear().noalias() = m_in.linear() + m_in.angular().cross(placement.translation());
64 return;
65 }
67 {
68 m_out.angular() = m_in.angular();
69 m_out.linear().noalias() = m_in.linear() - m_in.angular().cross(placement.translation());
70 return;
71 }
72
73 assert(false && "This must never happened.");
74 }
75
90 template<typename Scalar, int Options, typename MotionIn>
91 typename MotionIn::MotionPlain changeReferenceFrame(
92 const SE3Tpl<Scalar, Options> & placement,
93 const MotionDense<MotionIn> & m_in,
94 const ReferenceFrame rf_in,
95 const ReferenceFrame rf_out)
96 {
97 typedef typename MotionIn::MotionPlain ReturnType;
98 ReturnType res;
99
100 changeReferenceFrame(placement, m_in, rf_in, rf_out, res);
101
102 return res;
103 }
104
105} // namespace pinocchio
106
107#endif // #ifndef __pinocchio_multibody_fwd_hpp__
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition fwd.hpp:47
@ WORLD
Definition fwd.hpp:48
@ LOCAL_WORLD_ALIGNED
Definition fwd.hpp:52
@ LOCAL
Definition fwd.hpp:50
Main pinocchio namespace.
Definition treeview.dox:11
void changeReferenceFrame(const SE3Tpl< Scalar, Options > &placement, const ForceDense< ForceIn > &f_in, const ReferenceFrame rf_in, const ReferenceFrame rf_out, ForceDense< ForceOut > &f_out)
Definition force.hpp:19