DARTTypes.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014-2016 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #ifndef _GAZEBO_DARTTYPES_HH_
19 #define _GAZEBO_DARTTYPES_HH_
20 
21 #include <boost/shared_ptr.hpp>
22 #include "gazebo/common/Assert.hh"
23 #include "gazebo/math/Pose.hh"
25 #include "gazebo/util/system.hh"
26 
31 namespace gazebo
32 {
33  namespace physics
34  {
35  class DARTPhysics;
36  class DARTModel;
37  class DARTLink;
38  class DARTJoint;
39  class DARTCollision;
40  class DARTRayShape;
42 
43  typedef boost::shared_ptr<DARTPhysics> DARTPhysicsPtr;
44  typedef boost::shared_ptr<DARTModel> DARTModelPtr;
45  typedef boost::shared_ptr<DARTLink> DARTLinkPtr;
46  typedef boost::shared_ptr<DARTJoint> DARTJointPtr;
47  typedef boost::shared_ptr<DARTCollision> DARTCollisionPtr;
48  typedef boost::shared_ptr<DARTRayShape> DARTRayShapePtr;
49  typedef boost::shared_ptr<DARTSurfaceParams> DARTSurfaceParamsPtr;
50 
53 
58  {
60  public: static Eigen::Vector3d ConvVec3(const math::Vector3 &_vec3)
61  {
62  return Eigen::Vector3d(_vec3.x, _vec3.y, _vec3.z);
63  }
64 
66  public: static math::Vector3 ConvVec3(const Eigen::Vector3d &_vec3)
67  {
68  return math::Vector3(_vec3.x(), _vec3.y(), _vec3.z());
69  }
70 
72  public: static Eigen::Quaterniond ConvQuat(const math::Quaternion &_quat)
73  {
74  return Eigen::Quaterniond(_quat.w, _quat.x, _quat.y, _quat.z);
75  }
76 
78  public: static math::Quaternion ConvQuat(const Eigen::Quaterniond &_quat)
79  {
80  return math::Quaternion(_quat.w(), _quat.x(), _quat.y(), _quat.z());
81  }
82 
84  public: static Eigen::Isometry3d ConvPose(const math::Pose &_pose)
85  {
86  // Below line doesn't work with 'libeigen3-dev is 3.0.5-1'
87  // return Eigen::Translation3d(ConvVec3(_pose.pos)) *
88  // ConvQuat(_pose.rot);
89 
90  Eigen::Isometry3d res;
91 
92  res.translation() = ConvVec3(_pose.pos);
93  res.linear() = Eigen::Matrix3d(ConvQuat(_pose.rot));
94 
95  return res;
96  }
97 
99  public: static math::Pose ConvPose(const Eigen::Isometry3d &_T)
100  {
101  math::Pose pose;
102  pose.pos = ConvVec3(_T.translation());
103  pose.rot = ConvQuat(Eigen::Quaterniond(_T.linear()));
104  return pose;
105  }
106 
113  public: static double InvertThreadPitch(double _pitch)
114  {
115  GZ_ASSERT(std::abs(_pitch) > 0.0,
116  "Zero thread pitch is not allowed.\n");
117 
118  return -2.0 * M_PI / _pitch;
119  }
120  };
121  }
122 }
123 
124 #endif
static math::Vector3 ConvVec3(const Eigen::Vector3d &_vec3)
Definition: DARTTypes.hh:66
double z
z value of the quaternion
Definition: Quaternion.hh:386
#define GZ_PHYSICS_VISIBLE
Definition: system.hh:259
#define GZ_ASSERT(_expr, _msg)
This macro define the standard way of launching an exception inside gazebo.
Definition: Assert.hh:24
Encapsulates a position and rotation in three space.
Definition: Pose.hh:37
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
boost::shared_ptr< DARTPhysics > DARTPhysicsPtr
Definition: DARTTypes.hh:41
DART surface parameters.
Definition: DARTSurfaceParams.hh:39
double x
X location.
Definition: Vector3.hh:311
static Eigen::Quaterniond ConvQuat(const math::Quaternion &_quat)
Definition: DARTTypes.hh:72
static math::Pose ConvPose(const Eigen::Isometry3d &_T)
Definition: DARTTypes.hh:99
double z
Z location.
Definition: Vector3.hh:317
static math::Quaternion ConvQuat(const Eigen::Quaterniond &_quat)
Definition: DARTTypes.hh:78
double w
w value of the quaternion
Definition: Quaternion.hh:377
boost::shared_ptr< DARTModel > DARTModelPtr
Definition: DARTTypes.hh:44
boost::shared_ptr< DARTLink > DARTLinkPtr
Definition: DARTTypes.hh:45
Vector3 pos
The position.
Definition: Pose.hh:252
A quaternion class.
Definition: Quaternion.hh:42
Quaternion rot
The rotation.
Definition: Pose.hh:255
double y
y value of the quaternion
Definition: Quaternion.hh:383
static Eigen::Isometry3d ConvPose(const math::Pose &_pose)
Definition: DARTTypes.hh:84
double x
x value of the quaternion
Definition: Quaternion.hh:380
static Eigen::Vector3d ConvVec3(const math::Vector3 &_vec3)
Definition: DARTTypes.hh:60
boost::shared_ptr< DARTCollision > DARTCollisionPtr
Definition: DARTTypes.hh:47
boost::shared_ptr< DARTJoint > DARTJointPtr
Definition: DARTTypes.hh:46
boost::shared_ptr< DARTSurfaceParams > DARTSurfaceParamsPtr
Definition: DARTTypes.hh:49
A set of functions for converting between the math types used by gazebo and dart. ...
Definition: DARTTypes.hh:57
static double InvertThreadPitch(double _pitch)
Invert thread pitch to match the different definitions of thread pitch in Gazebo and DART...
Definition: DARTTypes.hh:113
double y
Y location.
Definition: Vector3.hh:314
boost::shared_ptr< DARTRayShape > DARTRayShapePtr
Definition: DARTTypes.hh:48