chain.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Stuart Glaser
31 
32 #ifndef MECHANISM_MODEL_CHAIN_H
33 #define MECHANISM_MODEL_CHAIN_H
34 
36 #include <kdl/chain.hpp>
37 #include <kdl/jntarray.hpp>
38 #include <kdl/jntarrayvel.hpp>
39 #include <kdl/jntarrayacc.hpp>
40 
42 
43 class Chain
44 {
45 public:
46  Chain() {}
47  ~Chain() {}
48 
56  bool init(RobotState *robot_state, const std::string &root, const std::string &tip);
57 
59  void getPositions(std::vector<double>&);
63  template <class Vec>
64  void getPositions(Vec &v)
65  {
66  assert((int)v.size() == (int)joints_.size());
67  for (size_t i = 0; i < joints_.size(); ++i)
68  v[i] = joints_[i]->position_;
69  }
70 
72  void getVelocities(std::vector<double>&);
76  template <class Vec>
77  void getVelocities(Vec &v)
78  {
79  assert((int)v.size() == (int)joints_.size());
80  for (size_t i = 0; i < joints_.size(); ++i)
81  v[i] = joints_[i]->velocity_;
82  }
83 
84 
86  void getEfforts(std::vector<double>&);
89 
94 
98  template <class Vec>
99  void addEfforts(const Vec& v)
100  {
101  assert((int)v.size() == (int)joints_.size());
102  for (size_t i = 0; i < joints_.size(); ++i)
103  joints_[i]->commanded_effort_ += v[i];
104  }
105 
107  bool allCalibrated();
108 
110  void toKDL(KDL::Chain &chain);
111 
117  JointState* getJoint(unsigned int actuated_joint_i);
118 
120  int size() const { return joints_.size(); }
121 
122 private:
125 
126  std::vector< JointState* > joints_; // ONLY joints that can be actuated (not fixed joints)
127 };
128 
129 }
130 
131 #endif
KDL::Chain kdl_chain_
Definition: chain.h:124
void getPositions(Vec &v)
gets the joint positions of the chain as any type with size() and []
Definition: chain.h:64
std::vector< JointState *> joints_
Definition: chain.h:126
void getPositions(std::vector< double > &)
get the joint positions of the chain as a std vector
Definition: chain.cpp:83
void setEfforts(KDL::JntArray &)
set the commanded joint efforts of the chain as a std vector
Definition: chain.cpp:150
void getVelocities(Vec &v)
gets the joint velocities of the chain as any type with size() and []
Definition: chain.h:77
void getVelocities(std::vector< double > &)
get the joint velocities of the chain as a std vector
Definition: chain.cpp:92
void addEfforts(const Vec &v)
Adds efforts from any type that implements size() and [] lookup.
Definition: chain.h:99
bool allCalibrated()
returns true if all the joints in the chain are calibrated
Definition: chain.cpp:110
pr2_mechanism_model::RobotState * robot_state_
Definition: chain.h:123
void getEfforts(std::vector< double > &)
get the measured joint efforts of the chain as a std vector
Definition: chain.cpp:101
This class provides the controllers with an interface to the robot state.
Definition: robot.h:126
int size() const
Returns the number of actuated joints in the chain.
Definition: chain.h:120
JointState * getJoint(unsigned int actuated_joint_i)
get a joint state of an actuated joint of the chain.
Definition: chain.cpp:165
bool init(RobotState *robot_state, const std::string &root, const std::string &tip)
initialize the chain object
Definition: chain.cpp:40
void toKDL(KDL::Chain &chain)
get a kdl chain object that respresents the chain from root to tip
Definition: chain.cpp:120
void addEfforts(KDL::JntArray &)
set the commanded joint efforts of the chain as a kdl jnt array
Definition: chain.cpp:157


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Tue Mar 7 2023 03:54:53