All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
JointController.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2012 Nate Koenig & Andrew Howard
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 _JOINTCONTROLLER_HH_
18 #define _JOINTCONTROLLER_HH_
19 
20 #include <map>
21 #include <string>
22 #include <vector>
23 
24 #include "gazebo/common/PID.hh"
25 #include "gazebo/common/Time.hh"
28 #include "gazebo/msgs/msgs.hh"
29 
30 namespace gazebo
31 {
32  namespace physics
33  {
36 
40  {
43  public: explicit JointController(ModelPtr _model);
44 
47  public: void AddJoint(JointPtr _joint);
48 
50  public: void Update();
51 
53  public: void Reset();
54 
57  public: void SetJointPosition(const std::string &_name, double _position);
58 
61  public: void SetJointPositions(
62  const std::map<std::string, double> &_jointPositions);
63 
66  private: void OnJointCmd(ConstJointCmdPtr &_msg);
67 
84  public: void SetJointPosition(JointPtr _joint, double _position);
85 
93  private: void MoveLinks(JointPtr _joint, LinkPtr _link,
94  const math::Vector3 &_anchor, const math::Vector3 &_axis,
95  double _dposition, bool _updateChildren = false);
96 
99  private: void ComputeAndSetLinkTwist(LinkPtr _link,
100  const math::Pose &_old, const math::Pose &_new, double dt);
101 
106  private: void AddConnectedLinks(std::vector<LinkPtr> &_linksOut,
107  const LinkPtr &_link,
108  bool _checkParentTree = false);
109 
113  private: template<class InputVector, class T>
114  bool ContainsLink(InputVector _vector, const T &_value)
115  {
116  typename InputVector::iterator iter = _vector.begin();
117  for (; iter != _vector.end(); ++iter)
118  if ((*iter)->GetScopedName() == _value->GetScopedName())
119  return true;
120  return false;
121  }
122 
124  private: ModelPtr model;
125 
127  private: Link_V updatedLinks;
128 
130  private: std::map<std::string, JointPtr> joints;
131 
133  private: std::map<std::string, common::PID> posPids;
134 
136  private: std::map<std::string, common::PID> velPids;
137 
139  private: std::map<std::string, double> forces;
140 
142  private: std::map<std::string, double> positions;
143 
145  private: std::map<std::string, double> velocities;
146 
148  private: transport::NodePtr node;
149 
151  private: transport::SubscriberPtr jointCmdSub;
152 
154  private: common::Time prevUpdateTime;
155  };
157  }
158 }
159 #endif