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 (C) 2012-2014 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 _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(
58  const std::string &_name, double _position, int _index = 0);
59 
62  public: void SetJointPositions(
63  const std::map<std::string, double> &_jointPositions);
64 
67  private: void OnJointCmd(ConstJointCmdPtr &_msg);
68 
85  public: void SetJointPosition(
86  JointPtr _joint, double _position, int _index = 0);
87 
95  private: void MoveLinks(JointPtr _joint, LinkPtr _link,
96  const math::Vector3 &_anchor, const math::Vector3 &_axis,
97  double _dposition, bool _updateChildren = false);
98 
101  private: void ComputeAndSetLinkTwist(LinkPtr _link,
102  const math::Pose &_old, const math::Pose &_new, double dt);
103 
108  private: void AddConnectedLinks(std::vector<LinkPtr> &_linksOut,
109  const LinkPtr &_link,
110  bool _checkParentTree = false);
111 
115  private: template<class InputVector, class T>
116  bool ContainsLink(InputVector _vector, const T &_value)
117  {
118  typename InputVector::iterator iter = _vector.begin();
119  for (; iter != _vector.end(); ++iter)
120  if ((*iter)->GetScopedName() == _value->GetScopedName())
121  return true;
122  return false;
123  }
124 
126  private: ModelPtr model;
127 
129  private: Link_V updatedLinks;
130 
132  private: std::map<std::string, JointPtr> joints;
133 
135  private: std::map<std::string, common::PID> posPids;
136 
138  private: std::map<std::string, common::PID> velPids;
139 
141  private: std::map<std::string, double> forces;
142 
144  private: std::map<std::string, double> positions;
145 
147  private: std::map<std::string, double> velocities;
148 
150  private: transport::NodePtr node;
151 
153  private: transport::SubscriberPtr jointCmdSub;
154 
156  private: common::Time prevUpdateTime;
157  };
159  }
160 }
161 #endif