pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
force.hpp
1//
2// Copyright (c) 2020 INRIA
3//
4
5#ifndef __pinocchio_algorithm_utils_force_hpp__
6#define __pinocchio_algorithm_utils_force_hpp__
7
8#include "pinocchio/spatial/se3.hpp"
9#include "pinocchio/spatial/force.hpp"
10#include "pinocchio/multibody/fwd.hpp"
11
12namespace pinocchio
13{
18 template<typename Scalar, int Options, typename ForceIn, typename ForceOut>
20 const SE3Tpl<Scalar, Options> & placement,
25 {
26 if (rf_in == rf_out)
27 {
28 f_out = f_in;
29 return;
30 }
31
32 // case: LOCAL/WORLD and WORLD/LOCAL
33 if (rf_in == LOCAL && rf_out == WORLD)
34 {
35 f_out = placement.act(f_in);
36 return;
37 }
38 if (rf_in == WORLD && rf_out == LOCAL)
39 {
40 f_out = placement.actInv(f_in);
41 return;
42 }
43
44 // case: LOCAL/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/LOCAL
46 {
47 f_out.angular().noalias() = placement.rotation() * f_in.angular();
48 f_out.linear().noalias() = placement.rotation() * f_in.linear();
49 return;
50 }
52 {
53 f_out.angular().noalias() = placement.rotation().transpose() * f_in.angular();
54 f_out.linear().noalias() = placement.rotation().transpose() * f_in.linear();
55 return;
56 }
57
58 // case: WORLD/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/WORLD
60 {
61 f_out.linear() = f_in.linear();
62 f_out.angular().noalias() = f_in.angular() + f_in.linear().cross(placement.translation());
63 return;
64 }
66 {
67 f_out.linear() = f_in.linear();
68 f_out.angular().noalias() = f_in.angular() - f_in.linear().cross(placement.translation());
69 return;
70 }
71
72 assert(false && "This must never happened.");
73 }
74
89 template<typename Scalar, int Options, typename ForceIn>
90 typename ForceIn::ForcePlain changeReferenceFrame(
91 const SE3Tpl<Scalar, Options> & placement,
92 const ForceDense<ForceIn> & f_in,
93 const ReferenceFrame rf_in,
94 const ReferenceFrame rf_out)
95 {
96 typedef typename ForceIn::ForcePlain ReturnType;
97 ReturnType res;
98
99 changeReferenceFrame(placement, f_in, rf_in, rf_out, res);
100
101 return res;
102 }
103
104} // namespace pinocchio
105
106#endif // #ifndef __pinocchio_algorithm_utils_force_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