1111
1212#include " standard_includes.h"
1313
14- /* ******************************************************************************************************************/ /* *
15- * Class defining a 3d position and rotation using Eigen Vector3 and Quaternion classes
16- ***********************************************************************************************************************/
17- class Pose
14+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
15+ // / Class defining a 3d position and rotation using Eigen Vector3 and Quaternion classes.
16+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1817{
1918public:
20-
21- /* *
22- * Blank contructor
23- */
19+ // / Blank contructor
2420 inline Pose (void ) {};
2521
26- /* *
27- * Pose class constructor
28- */
22+ // / Pose class constructor.
23+ // / @param[in] position The vector input to be set as the position vector of the pose
24+ // / @param[in] rotation The vector input to be set as the rotation vector of the pose
2925 inline Pose (const Vector3d& position, const Quaterniond& rotation)
3026 {
3127 position_ = position;
3228 rotation_ = rotation;
3329 };
3430
35- /* *
36- * Pose class constructor for geometry_msgs/Pose
37- */
31+ // / Pose class constructor for geometry_msgs/Pose.
32+ // / @param[in] pose The geometry_msgs/Pose input to be set as the pose
3833 inline Pose (const geometry_msgs::Pose& pose)
3934 {
4035 position_ = Vector3d (pose.position .x , pose.position .y , pose.position .z );
4136 rotation_ = Quaterniond (pose.orientation .w , pose.orientation .x , pose.orientation .y , pose.orientation .z );
4237 };
4338
44- /* *
45- * Checks for NaN values within pose elements
46- * @return Bool denoting whether pose contains no NaN values
47- */
39+ // / Checks for NaN values within pose elements.
40+ // / @return Bool denoting whether pose contains no NaN values
4841 inline bool isValid (void )
4942 {
5043 return abs (position_[0 ]) < UNASSIGNED_VALUE &&
@@ -56,19 +49,16 @@ class Pose
5649 abs (rotation_.z ()) < UNASSIGNED_VALUE;
5750 }
5851
59- /* *
60- * Pose class constructor for geometry_msgs/Transform
61- */
52+ // / Pose class constructor for geometry_msgs/Transform.
53+ // / @param[in] transform The geometry_msgs/Transform input to be used to construct the pose
6254 inline Pose (const geometry_msgs::Transform& transform)
6355 {
6456 position_ = Vector3d (transform.translation .x , transform.translation .y , transform.translation .z );
6557 rotation_ = Quaterniond (transform.rotation .w , transform.rotation .x , transform.rotation .y , transform.rotation .z );
6658 };
6759
68- /* *
69- * Returns a conversion of this pose object into a geometry_msgs::Pose
70- * @return The converted geometry_msgs::Pose message
71- */
60+ // / Returns a conversion of this pose object into a geometry_msgs::Pose.
61+ // / @return The converted geometry_msgs::Pose message
7262 inline geometry_msgs::Pose toPoseMessage (void )
7363 {
7464 geometry_msgs::Pose pose;
@@ -82,10 +72,8 @@ class Pose
8272 return pose;
8373 }
8474
85- /* *
86- * Returns a conversion of this pose object into a geometry_msgs::Transform
87- * @return The converted geometry_msgs::Transform message
88- */
75+ // / Returns a conversion of this pose object into a geometry_msgs::Transform.
76+ // / @return The converted geometry_msgs::Transform message
8977 inline geometry_msgs::Transform toTransformMessage (void )
9078 {
9179 geometry_msgs::Transform transform;
@@ -99,40 +87,32 @@ class Pose
9987 return transform;
10088 }
10189
102- /* *
103- * Operator to check if two poses are equivalent
104- * @params[in] pose The pose that is checked for equivalency against *this
105- * @return Bool defining if input and *this pose are equivalent.
106- */
90+ // / Operator to check if two poses are equivalent.
91+ // / @param[in] pose The pose that is checked for equivalency against *this
92+ // / @return Bool defining if input and *this pose are equivalent
10793 inline bool operator ==(const Pose& pose)
10894 {
10995 return position_.isApprox (pose.position_ ) && rotation_.isApprox (pose.rotation_ );
11096 }
11197
112- /* *
113- * Operator to check if two poses are NOT equivalent
114- * @params[in] pose The pose that is checked for non-equivalency against *this
115- * @return Bool defining if input and *this pose are non-equivalent.
116- */
98+ // / Operator to check if two poses are NOT equivalent.
99+ // / @param[in] pose The pose that is checked for non-equivalency against *this
100+ // / @return Bool defining if input and *this pose are non-equivalent
117101 inline bool operator !=(const Pose& pose)
118102 {
119103 return !position_.isApprox (pose.position_ ) || !rotation_.isApprox (pose.rotation_ );
120104 }
121105
122- /* *
123- * Returns inverse of pose
124- * @return The inverse of *this pose
125- */
106+ // / Returns inverse of pose.
107+ // / @return The inverse of *this pose
126108 inline Pose operator ~(void ) const
127109 {
128110 return Pose ((rotation_.conjugate ())._transformVector (-position_), rotation_.conjugate ());
129111 }
130112
131- /* *
132- * Transforms this pose according to an input geometry_msgs::Transform
133- * @params[in] transform The input transformation msg
134- * @return The transformed pose
135- */
113+ // / Transforms this pose according to an input geometry_msgs::Transform.
114+ // / @param[in] transform The input transformation msg
115+ // / @return The transformed pose
136116 inline Pose transform (const geometry_msgs::Transform& transform) const
137117 {
138118 Vector3d position (position_ + Vector3d (transform.translation .x ,
@@ -145,11 +125,9 @@ class Pose
145125 return Pose (position, rotation);
146126 }
147127
148- /* *
149- * Transforms this pose according to an input transformation matrix constructed from DH matrices
150- * @params[in] transform The input transformation matrix
151- * @return The transformed pose
152- */
128+ // / Transforms this pose according to an input transformation matrix constructed from DH matrices.
129+ // / @param[in] transform The input transformation matrix
130+ // / @return The transformed pose
153131 inline Pose transform (const Matrix4d& transform) const
154132 {
155133 Pose return_pose;
@@ -163,31 +141,25 @@ class Pose
163141 return return_pose;
164142 }
165143
166- /* *
167- * Transforms an input vector into the reference frame of this pose.
168- * @params[in] vec The input vector to be transformed into this pose's reference frame.
169- * @return The transformed vector.
170- */
144+ // / Transforms an input vector into the reference frame of this pose.
145+ // / @param[in] vec The input vector to be transformed into this pose's reference frame
146+ // / @return The transformed vector
171147 inline Vector3d transformVector (const Vector3d& vec) const
172148 {
173149 return position_ + rotation_._transformVector (vec);
174150 };
175151
176- /* *
177- * Transforms an input vector from the reference frame of this pose.
178- * @params[in] vec The input vector to be transformed from this pose's reference frame.
179- * @return The inversly transformed vector
180- */
152+ // / Transforms an input vector from the reference frame of this pose.
153+ // / @param[in] vec The input vector to be transformed from this pose's reference frame
154+ // / @return The inversly transformed vector
181155 inline Vector3d inverseTransformVector (const Vector3d& vec) const
182156 {
183157 return (~*this ).transformVector (vec);
184158 };
185159
186- /* *
187- * Adds input pose to *this pose
188- * @params[in] pose The pose to add from *this pose
189- * @return The combination of *this pose and input pose
190- */
160+ // / Adds input pose to *this pose..
161+ // / @param[in] pose The pose to add from *this pose
162+ // / @return The combination of *this pose and input pose
191163 inline Pose addPose (const Pose& pose)
192164 {
193165 Pose return_pose = (*this );
@@ -196,11 +168,9 @@ class Pose
196168 return return_pose;
197169 }
198170
199- /* *
200- * Removes input pose from *this pose
201- * @params[in] pose The pose to remove from *this pose
202- * @return The resultant pose after removing input pose from *this pose
203- */
171+ // / Removes input pose from *this pose.
172+ // / @param[in] pose The pose to remove from *this pose
173+ // / @return The resultant pose after removing input pose from *this pose
204174 inline Pose removePose (const Pose& pose)
205175 {
206176 Pose return_pose = (*this );
@@ -209,32 +179,26 @@ class Pose
209179 return return_pose;
210180 };
211181
212- /* *
213- * Generates interpolation from this pose to target pose using control input between zero and one.
214- * @params[in] control_input A value between 0.0 and 1.0 which defines the progress of interpolation.
215- * @params[in] target_pose The target pose to which interpolation will return with control input of one.
216- * @return The resultant interpolated pose.
217- */
182+ // / Generates interpolation from this pose to target pose using control input between zero and one.
183+ // / @param[in] control_input A value between 0.0 and 1.0 which defines the progress of interpolation
184+ // / @param[in] target_pose The target pose to which interpolation will return with control input of one
185+ // / @return The resultant interpolated pose
218186 inline Pose interpolate (const double & control_input, const Pose& target_pose)
219187 {
220188 Vector3d position = control_input * target_pose.position_ + (1.0 - control_input) * (*this ).position_ ;
221189 Quaterniond rotation = (*this ).rotation_ .slerp (control_input, target_pose.rotation_ );
222190 return Pose (position, rotation);
223191 };
224192
225- /* *
226- * Returns pose with position and rotation elements set to identity values.
227- * @return The Identity Pose
228- */
193+ // / Returns pose with position and rotation elements set to identity values.
194+ // / @return The Identity Pose
229195 inline static Pose Identity (void )
230196 {
231197 return Pose (Vector3d::Zero (), Quaterniond::Identity ());
232198 }
233199
234- /* *
235- * Returns pose with position and rotation elements all set to undefined values.
236- * @return The Undefined Pose
237- */
200+ // / Returns pose with position and rotation elements all set to undefined values.
201+ // / @return The Undefined Pose
238202 inline static Pose Undefined (void )
239203 {
240204 return Pose (UNDEFINED_POSITION, UNDEFINED_ROTATION);
@@ -247,6 +211,6 @@ class Pose
247211 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
248212};
249213
250- /* **********************************************************************************************************************
251- ********************************************************************************************************************** */
252- #endif /* SYROPOD_HIGHLEVEL_CONTROLLER_POSE_H */
214+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215+
216+ #endif // SYROPOD_HIGHLEVEL_CONTROLLER_POSE_H
0 commit comments