SimbodyLink.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 #ifndef GAZEBO_PHYSICS_SIMBODY_SIMBODYLINK_HH_
18 #define GAZEBO_PHYSICS_SIMBODY_SIMBODYLINK_HH_
19 
20 #include <vector>
21 
23 #include "gazebo/physics/Link.hh"
24 
26 #include "gazebo/util/system.hh"
27 
28 namespace gazebo
29 {
30  namespace physics
31  {
36 
38  class GZ_PHYSICS_VISIBLE SimbodyLink : public Link
39  {
41  public: explicit SimbodyLink(EntityPtr _parent);
42 
44  public: virtual ~SimbodyLink();
45 
46  // Documentation inherited.
47  public: virtual void Load(sdf::ElementPtr _ptr);
48 
49  // Documentation inherited.
50  public: virtual void Init();
51 
52  // Documentation inherited.
53  public: virtual void Fini();
54 
55  // Documentation inherited.
56  public: virtual void OnPoseChange();
57 
58  // Documentation inherited.
59  public: virtual void SetEnabled(bool enable) const;
60 
61  // Documentation inherited.
62  public: virtual bool GetEnabled() const;
63 
64  // Documentation inherited.
65  public: virtual void SetLinearVel(const ignition::math::Vector3d &_vel);
66 
67  // Documentation inherited.
68  public: virtual void SetAngularVel(const ignition::math::Vector3d &_vel);
69 
70  // Documentation inherited.
71  public: virtual void SetForce(const ignition::math::Vector3d &_force);
72 
73  // Documentation inherited.
74  public: virtual void SetTorque(const ignition::math::Vector3d &_force);
75 
76  // Documentation inherited.
77  public: virtual ignition::math::Vector3d WorldLinearVel(
78  const ignition::math::Vector3d &_vector3) const;
79 
80  // Documentation inherited.
81  public: virtual ignition::math::Vector3d WorldLinearVel(
82  const ignition::math::Vector3d &_offset,
83  const ignition::math::Quaterniond &_q) const;
84 
85  // Documentation inherited.
86  public: virtual ignition::math::Vector3d WorldCoGLinearVel() const;
87 
88  // Documentation inherited.
89  public: virtual ignition::math::Vector3d WorldAngularVel() const;
90 
91  // Documentation inherited.
92  public: virtual ignition::math::Vector3d WorldForce() const;
93 
94  // Documentation inherited.
95  public: virtual ignition::math::Vector3d WorldTorque() const;
96 
97  // Documentation inherited.
98  public: virtual void SetGravityMode(bool _mode);
99 
100  // Documentation inherited.
101  public: virtual bool GetGravityMode() const;
102 
103  // Documentation inherited.
104  public: virtual void SetSelfCollide(bool _collide);
105 
106  // Documentation inherited.
107  public: virtual void SetLinearDamping(double _damping);
108 
109  // Documentation inherited.
110  public: virtual void SetAngularDamping(double _damping);
111 
112  // Documentation inherited.
113  public: virtual void AddForce(const ignition::math::Vector3d &_force);
114 
115  // Documentation inherited.
116  public: virtual void AddRelativeForce(
117  const ignition::math::Vector3d &_force);
118 
119  // Documentation inherited.
120  public: virtual void AddForceAtWorldPosition(
121  const ignition::math::Vector3d &_force,
122  const ignition::math::Vector3d &_pos);
123 
124  // Documentation inherited.
125  public: virtual void AddForceAtRelativePosition(
126  const ignition::math::Vector3d &_force,
127  const ignition::math::Vector3d &_relpos);
128 
129  // Documentation inherited
130  public: virtual void AddLinkForce(const ignition::math::Vector3d &_force,
131  const ignition::math::Vector3d &_offset =
132  ignition::math::Vector3d::Zero);
133 
134  // Documentation inherited.
135  public: virtual void AddTorque(const ignition::math::Vector3d &_torque);
136 
137  // Documentation inherited.
138  public: virtual void AddRelativeTorque(
139  const ignition::math::Vector3d &_torque);
140 
141  // Documentation inherited.
142  public: virtual void SetAutoDisable(bool _disable);
143 
144  // Documentation inherited.
145  public: virtual void SaveSimbodyState(const SimTK::State &_state);
146 
147  // Documentation inherited.
148  public: virtual void RestoreSimbodyState(SimTK::State &_state);
149 
155  public: virtual void SetLinkStatic(bool _static);
156 
160  public: SimTK::MassProperties GetMassProperties() const;
161 
162  public: SimTK::MassProperties GetEffectiveMassProps(
163  int _numFragments) const;
164 
167  public: void SetDirtyPose(const ignition::math::Pose3d &_pose);
168 
169  // Documentation inherited.
170  public: virtual void UpdateMass();
171 
174  private: void ProcessSetGravityMode();
175 
178  private: void ProcessSetLinkStatic();
179 
182  public: bool mustBeBaseLink;
183 
184  // Below to be filled in after everything is loaded
185  // Which MobilizedBody corresponds to the master instance of this link.
186  public: SimTK::MobilizedBody masterMobod;
187 
188  // Keeps track if physics has been initialized
189  public: bool physicsInitialized;
190 
191  // If this link got split into a master and slaves, these are the
192  // MobilizedBodies used to mobilize the slaves.
193  public: std::vector<SimTK::MobilizedBody> slaveMobods;
194 
195  // And these are the Weld constraints used to attach slaves to master.
196  public: std::vector<SimTK::Constraint::Weld> slaveWelds;
197 
199  private: bool gravityMode;
200 
202  private: bool staticLinkDirty;
203 
205  private: bool gravityModeDirty;
206 
208  private: bool staticLink;
209 
211  private: event::ConnectionPtr staticLinkConnection;
212 
214  private: event::ConnectionPtr gravityModeConnection;
215 
217  private: std::vector<double> simbodyQ;
218 
220  private: std::vector<double> simbodyU;
221 
223  private: SimbodyPhysicsPtr simbodyPhysics;
224  };
226  }
227 }
228 #endif
void enable()
Enable sensors.
boost::shared_ptr< SimbodyPhysics > SimbodyPhysicsPtr
Definition: SimbodyTypes.hh:40
Simbody wrapper forward declarations and typedefs.
boost::shared_ptr< Connection > ConnectionPtr
Definition: CommonTypes.hh:134
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:85
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.