forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdrakeGeometryUtil.h
206 lines (166 loc) · 8.07 KB
/
drakeGeometryUtil.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
#ifndef __DRAKE_GEOMETRY_UTIL_H__
#define __DRAKE_GEOMETRY_UTIL_H__
#include <Eigen/Dense>
#include <cstring>
#include <cmath>
#include <random>
#include "drakeGradientUtil.h"
#undef DLLEXPORT
#if defined(WIN32) || defined(WIN64)
#if defined(drakeGeometryUtil_EXPORTS)
#define DLLEXPORT __declspec( dllexport )
#else
#define DLLEXPORT __declspec( dllimport )
#endif
#else
#define DLLEXPORT
#endif
const int TWIST_SIZE = 6;
const int QUAT_SIZE = 4;
const int HOMOGENEOUS_TRANSFORM_SIZE = 16;
const int AXIS_ANGLE_SIZE = 4;
const int SPACE_DIMENSION = 3;
const int RotmatSize = SPACE_DIMENSION * SPACE_DIMENSION;
const int RPY_SIZE = 3;
DLLEXPORT double angleDiff(double phi1, double phi2);
/*
* quaternion methods
*/
DLLEXPORT Eigen::Vector4d quatConjugate(const Eigen::Vector4d &q);
DLLEXPORT Eigen::Matrix4d dquatConjugate();
DLLEXPORT Eigen::Vector4d quatProduct(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2);
DLLEXPORT Eigen::Matrix<double, 4, 8> dquatProduct(const Eigen::Vector4d &q1,const Eigen::Vector4d &q2);
DLLEXPORT Eigen::Vector3d quatRotateVec(const Eigen::Vector4d &q, const Eigen::Vector3d &v);
DLLEXPORT Eigen::Matrix<double, 3, 7> dquatRotateVec(const Eigen::Vector4d &q, const Eigen::Vector3d &v);
DLLEXPORT Eigen::Vector4d quatDiff(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2);
DLLEXPORT Eigen::Matrix<double, 4, 8> dquatDiff(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2);
DLLEXPORT double quatDiffAxisInvar(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, const Eigen::Vector3d &u);
DLLEXPORT Eigen::Matrix<double, 1, 11> dquatDiffAxisInvar(const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, const Eigen::Vector3d &u);
DLLEXPORT double quatNorm(const Eigen::Vector4d& q);
DLLEXPORT Eigen::Vector4d uniformlyRandomAxisAngle(std::default_random_engine& generator);
DLLEXPORT Eigen::Vector4d uniformlyRandomQuat(std::default_random_engine& generator);
DLLEXPORT Eigen::Matrix3d uniformlyRandomRotmat(std::default_random_engine& generator);
DLLEXPORT Eigen::Vector3d uniformlyRandomRPY(std::default_random_engine& generator);
/*
* quat2x
*/
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 4, 1> quat2axis(const Eigen::MatrixBase<Derived>& q);
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 3> quat2rotmat(const Eigen::MatrixBase<Derived>& q);
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 1> quat2rpy(const Eigen::MatrixBase<Derived>& q);
/*
* axis2x
*/
template <typename Derived>
DLLEXPORT Eigen::Vector4d axis2quat(const Eigen::MatrixBase<Derived>& a);
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 3> axis2rotmat(const Eigen::MatrixBase<Derived>& a);
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 1> axis2rpy(const Eigen::MatrixBase<Derived>& a);
/*
* rotmat2x
*/
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 4, 1> rotmat2axis(const Eigen::MatrixBase<Derived>& R);
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 4, 1> rotmat2quat(const Eigen::MatrixBase<Derived>& M);
template<typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 1> rotmat2rpy(const Eigen::MatrixBase<Derived>& R);
/*
* rpy2x
*/
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 4, 1> rpy2axis(const Eigen::MatrixBase<Derived>& rpy);
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 4, 1> rpy2quat(const Eigen::MatrixBase<Derived>& rpy);
template<typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 3> rpy2rotmat(const Eigen::MatrixBase<Derived>& rpy);
template <typename Derived>
DLLEXPORT Eigen::Matrix<typename Derived::Scalar, 3, 3> vectorToSkewSymmetric(const Eigen::MatrixBase<Derived>& p);
/*
* rotation conversion gradient functions
*/
template <typename Derived>
DLLEXPORT void normalizeVec(
const Eigen::MatrixBase<Derived>& x,
typename Derived::PlainObject& x_norm,
typename Gradient<Derived, Derived::RowsAtCompileTime, 1>::type* dx_norm = nullptr,
typename Gradient<Derived, Derived::RowsAtCompileTime, 2>::type* ddx_norm = nullptr);
template <typename Derived>
DLLEXPORT typename Gradient<Eigen::Matrix<typename Derived::Scalar, 3, 3>, QUAT_SIZE>::type dquat2rotmat(const Eigen::MatrixBase<Derived>& q);
template <typename DerivedR, typename DerivedDR>
DLLEXPORT typename Gradient<Eigen::Matrix<typename DerivedR::Scalar, RPY_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type drotmat2rpy(
const Eigen::MatrixBase<DerivedR>& R,
const Eigen::MatrixBase<DerivedDR>& dR);
template <typename DerivedR, typename DerivedDR>
DLLEXPORT typename Gradient<Eigen::Matrix<typename DerivedR::Scalar, QUAT_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type drotmat2quat(
const Eigen::MatrixBase<DerivedR>& R,
const Eigen::MatrixBase<DerivedDR>& dR);
/*
* angular velocity conversion functions
*/
template <typename DerivedQ, typename DerivedM>
DLLEXPORT void angularvel2quatdotMatrix(const Eigen::MatrixBase<DerivedQ>& q,
Eigen::MatrixBase<DerivedM>& M,
typename Gradient<DerivedM, QUAT_SIZE, 1>::type* dM = nullptr);
template<typename DerivedRPY, typename DerivedPhi>
DLLEXPORT void angularvel2rpydotMatrix(const Eigen::MatrixBase<DerivedRPY>& rpy,
typename Eigen::MatrixBase<DerivedPhi>& phi,
typename Gradient<DerivedPhi, RPY_SIZE, 1>::type* dphi = nullptr,
typename Gradient<DerivedPhi, RPY_SIZE, 2>::type* ddphi = nullptr);
template<typename DerivedRPY, typename DerivedE>
DLLEXPORT void rpydot2angularvelMatrix(const Eigen::MatrixBase<DerivedRPY>& rpy,
Eigen::MatrixBase<DerivedE>& E,
typename Gradient<DerivedE,RPY_SIZE,1>::type* dE=nullptr);
template <typename DerivedQ, typename DerivedM>
DLLEXPORT void quatdot2angularvelMatrix(const Eigen::MatrixBase<DerivedQ>& q,
Eigen::MatrixBase<DerivedM>& M,
typename Gradient<DerivedM, QUAT_SIZE, 1>::type* dM = nullptr);
//#endif
/*
* spatial transform functions
*/
template<typename Derived>
struct TransformSpatial {
typedef typename Eigen::Matrix<typename Derived::Scalar, TWIST_SIZE, Derived::ColsAtCompileTime> type;
};
template<typename DerivedM>
DLLEXPORT typename TransformSpatial<DerivedM>::type transformSpatialMotion(
const Eigen::Transform<typename DerivedM::Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedM>& M);
template<typename DerivedF>
DLLEXPORT typename TransformSpatial<DerivedF>::type transformSpatialForce(
const Eigen::Transform<typename DerivedF::Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedF>& F);
//#if !defined(WIN32) && !defined(WIN64)
/*
* spatial transform gradient methods
*/
template<typename DerivedQdotToV>
struct DHomogTrans {
typedef typename Eigen::Matrix<typename DerivedQdotToV::Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedQdotToV::ColsAtCompileTime> type;
};
template<typename DerivedS, typename DerivedQdotToV>
DLLEXPORT typename DHomogTrans<DerivedQdotToV>::type dHomogTrans(
const Eigen::Transform<typename DerivedQdotToV::Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedS>& S,
const Eigen::MatrixBase<DerivedQdotToV>& qdot_to_v);
template<typename DerivedDT>
DLLEXPORT typename DHomogTrans<DerivedDT>::type dHomogTransInv(
const Eigen::Transform<typename DerivedDT::Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedDT>& dT);
template <typename Scalar, typename DerivedX, typename DerivedDT, typename DerivedDX>
DLLEXPORT typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime, 1>::type dTransformAdjoint(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedX>& X,
const Eigen::MatrixBase<DerivedDT>& dT,
const Eigen::MatrixBase<DerivedDX>& dX);
template <typename Scalar, typename DerivedX, typename DerivedDT, typename DerivedDX>
DLLEXPORT typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type dTransformAdjointTranspose(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedX>& X,
const Eigen::MatrixBase<DerivedDT>& dT,
const Eigen::MatrixBase<DerivedDX>& dX);
#endif