SimbodyPhysics.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 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_PHYSICS_SIMBODY_SIMBODYPHYSICS_HH
19 #define GAZEBO_PHYSICS_SIMBODY_SIMBODYPHYSICS_HH
20 #include <string>
21 
22 #include <boost/thread/thread.hpp>
23 #include <boost/thread/mutex.hpp>
24 
27 #include "gazebo/physics/Shape.hh"
29 
31 #include "gazebo/util/system.hh"
32 
33 namespace gazebo
34 {
35  namespace physics
36  {
40 
42  class GZ_PHYSICS_VISIBLE SimbodyPhysics : public PhysicsEngine
43  {
45  public: explicit SimbodyPhysics(WorldPtr _world);
46 
48  public: virtual ~SimbodyPhysics();
49 
50  // Documentation inherited
51  public: virtual void Load(sdf::ElementPtr _sdf);
52 
53  // Documentation inherited
54  public: virtual void Init();
55 
56  // Documentation inherited
57  public: virtual void Reset();
58 
61  public: void InitModel(const physics::ModelPtr _model);
62 
63  // Documentation inherited
64  public: virtual void InitForThread();
65 
66  // Documentation inherited
67  public: virtual void UpdateCollision();
68 
69  // Documentation inherited
70  public: virtual void UpdatePhysics();
71 
72  // Documentation inherited
73  public: virtual void Fini();
74 
75  // Documentation inherited
76  public: virtual std::string GetType() const;
77 
78  // Documentation inherited
79  public: virtual LinkPtr CreateLink(ModelPtr _parent);
80 
81  // Documentation inherited
82  public: virtual CollisionPtr CreateCollision(const std::string &_type,
83  LinkPtr _body);
84 
85  // Documentation inherited
86  public: virtual JointPtr CreateJoint(const std::string &_type,
87  ModelPtr _parent);
88 
89  // Documentation inherited
90  public: virtual ShapePtr CreateShape(const std::string &_shapeType,
91  CollisionPtr _collision);
92 
94  public: SimTK::MultibodySystem *GetDynamicsWorld() const;
95 
96  // Documentation inherited
97  public: virtual void SetGravity(const ignition::math::Vector3d &_gravity);
98 
99  // Documentation inherited
100  public: virtual void DebugPrint() const;
101 
102  // Documentation inherited
103  public: virtual void SetSeed(uint32_t _seed);
104 
105  // Documentation inherited
106  public: virtual ModelPtr CreateModel(BasePtr _parent);
107 
111  public: static SimTK::Quaternion QuadToQuad(
112  const ignition::math::Quaterniond &_q);
113 
117  public: static ignition::math::Quaterniond QuadToQuadIgn(
118  const SimTK::Quaternion &_q);
119 
123  public: static SimTK::Vec3 Vector3ToVec3(
124  const ignition::math::Vector3d &_v);
125 
129  public: static ignition::math::Vector3d Vec3ToVector3Ign(
130  const SimTK::Vec3 &_v);
131 
138  public: static SimTK::Transform Pose2Transform(
139  const ignition::math::Pose3d &_pose);
140 
145  public: static ignition::math::Pose3d Transform2PoseIgn(
146  const SimTK::Transform &_xAB);
147 
151  public: static SimTK::Transform GetPose(sdf::ElementPtr _element);
152 
157  public: static std::string GetTypeString(unsigned int _type);
158 
163  public: static std::string GetTypeString(physics::Base::EntityType _type);
164 
165  // Documentation inherited
166  protected: virtual void OnRequest(ConstRequestPtr &_msg);
167 
168  // Documentation inherited
169  protected: virtual void OnPhysicsMsg(ConstPhysicsPtr &_msg);
170 
179  private: void CreateMultibodyGraph(
180  SimTK::MultibodyGraphMaker& _mbgraph, const physics::ModelPtr _model);
181 
192  private: void InitSimbodySystem();
193 
196  private: void AddStaticModelToSimbodySystem(
197  const physics::ModelPtr _model);
198 
202  private: void AddDynamicModelToSimbodySystem(
203  const SimTK::MultibodyGraphMaker &_mbgraph,
204  const physics::ModelPtr _model);
205 
207  private: void AddCollisionsToLink(const physics::SimbodyLink *_link,
208  SimTK::MobilizedBody &_mobod, SimTK::ContactCliqueId _modelClique);
209 
210  public: SimTK::MultibodySystem system;
211  public: SimTK::SimbodyMatterSubsystem matter;
212  public: SimTK::GeneralForceSubsystem forces;
213  public: SimTK::Force::Gravity gravity;
214  public: SimTK::Force::DiscreteForces discreteForces;
215  public: SimTK::ContactTrackerSubsystem tracker;
216  public: SimTK::CompliantContactSubsystem contact;
217  public: SimTK:: Integrator *integ;
218 
221 
222  public: bool simbodyPhysicsStepped;
223 
224  // Documentation inherited
225  public: virtual boost::any GetParam(const std::string &_key) const;
226 
227  // Documentation inherited
228  public: virtual bool GetParam(const std::string &_key,
229  boost::any &_value) const;
230 
231  // Documentation inherited
232  public: virtual bool SetParam(const std::string &_key,
233  const boost::any &_value);
234 
236  private: double contactMaterialStiffness;
237 
239  private: double contactMaterialDissipation;
240 
243  private: double contactMaterialPlasticCoefRestitution;
244 
247  private: double contactMaterialPlasticImpactVelocity;
248 
251  private: double contactMaterialStaticFriction;
252 
255  private: double contactMaterialDynamicFriction;
256 
259  private: double contactMaterialViscousFriction;
260 
263  private: double contactImpactCaptureVelocity;
264 
267  private: double contactStictionTransitionVelocity;
268 
269  private: SimTK::MultibodySystem *dynamicsWorld;
270 
271  private: common::Time lastUpdateTime;
272 
273  private: double stepTimeDouble;
274 
277  private: std::string solverType;
278 
284  private: std::string integratorType;
285  };
287  }
288 }
289 #endif
SimTK::Integrator * integ
Definition: SimbodyPhysics.hh:217
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
boost::shared_ptr< World > WorldPtr
Definition: PhysicsTypes.hh:89
SimTK::Force::DiscreteForces discreteForces
Definition: SimbodyPhysics.hh:214
SimTK::SimbodyMatterSubsystem matter
Definition: SimbodyPhysics.hh:211
Base class for a physics engine.
Definition: PhysicsEngine.hh:41
bool simbodyPhysicsStepped
Definition: SimbodyPhysics.hh:222
boost::shared_ptr< Joint > JointPtr
Definition: PhysicsTypes.hh:117
Simbody physics engine.
Definition: SimbodyPhysics.hh:42
SimTK::CompliantContactSubsystem contact
Definition: SimbodyPhysics.hh:216
SimTK::MultibodySystem system
Definition: SimbodyPhysics.hh:210
Simbody wrapper forward declarations and typedefs.
SimTK::Force::Gravity gravity
Definition: SimbodyPhysics.hh:213
boost::shared_ptr< Shape > ShapePtr
Definition: PhysicsTypes.hh:141
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
EntityType
Unique identifiers for all entity types.
Definition: Base.hh:81
SimTK::GeneralForceSubsystem forces
Definition: SimbodyPhysics.hh:212
bool simbodyPhysicsInitialized
true if initialized
Definition: SimbodyPhysics.hh:220
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113
SimTK::ContactTrackerSubsystem tracker
Definition: SimbodyPhysics.hh:215
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:77
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44