HEBI C++ API  3.9.0
end_effector.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include "lookup.hpp"
4 #include "group.hpp"
5 
6 namespace hebi {
7 namespace experimental {
8 namespace arm {
9 
10 // This is a general end effector that can be added to the end of the arm.
11 // Override this class to create end effectors for particular purposes.
13 public:
14  EndEffectorBase() = default;
15  // Updates feedback and sets aux state.
16  virtual bool update(Eigen::VectorXd& aux_state) { return true; }
17  // Sends command
18  virtual bool send() { return true; }
19 };
20 
21 // An end effector with "n" commandable states.
22 // Override this abstract base class to create a fully functioning end
23 // effector (e.g., the "EffortEndEffector" below).
24 template <size_t AuxSize>
25 class EndEffector : public EndEffectorBase {
26 
27 public:
28  // Updates feedback and sets aux state.
29  // Note: parallel grippers should have aux_state size of AuxSize or 0.
30  // State of size "0" indicates no change. Values of "nan" also indicate
31  // no change.
32  // Invalid inputs result in a "false" return value, with no
33  // command set.
34  bool update(Eigen::VectorXd& aux_state) override {
35  // Check for valid aux state:
36  auto n = aux_state.size();
37  if (n != AuxSize && n != 0)
38  return false;
39 
40  // Set aux state when given:
41  if (aux_state.size() == AuxSize)
42  {
43  for (size_t i = 0; i < AuxSize; ++i)
44  {
45  if (!std::isnan(aux_state[i]))
46  setCommand(i, aux_state[i]);
47  }
48  }
49 
50  return group_->getNextFeedback(feedback_);
51  }
52 
53  // Sends command to gripper.
54  bool send() override {
55  return group_->sendCommand(command_);
56  }
57 
58  // Loads gains from the given .xml file, and sends them to the gripper group.
59  // Returns false if the gains file could not be found, if these is a mismatch
60  // in number of modules, or the modules do not acknowledge receipt of the
61  // gains.
62  bool loadGains(const std::string& gains_file) {
63  hebi::GroupCommand gains_cmd(group_->size());
64  if (!gains_cmd.readGains(gains_file))
65  return false;
66 
67  return group_->sendCommandWithAcknowledgement(gains_cmd);
68  }
69 
70 protected:
71  // Typed setters depending on class type
72  virtual void setCommand(size_t index, double value) = 0;
73 
74  static std::shared_ptr<hebi::Group> getGroup(const std::vector<std::string>& families, const std::vector<std::string>& names) {
75  Lookup lookup;
76  auto group = lookup.getGroupFromNames(families, names);
77  if (!group || group->size() != AuxSize)
78  return nullptr;
79  return group;
80  }
81 
82  EndEffector(std::shared_ptr<hebi::Group> group) : command_(AuxSize), group_(group), feedback_(AuxSize) { }
83 
85 
86 private:
87  std::shared_ptr<hebi::Group> group_;
88  hebi::GroupFeedback feedback_;
89 };
90 
91 // A general effort-controlled gripper, sending commands to HEBI modules. This
92 // templated class can work with "n" modules, although "1" is the most common case.
93 //
94 // Example usage for effort-controller gripper:
95 //
96 // auto end_effector = EndEffector<1, EndEffectorCommand::Effort>::create("Arm", "gripperSpool");
97 // Eigen::VectorXd aux_state(1);
98 // aux_state.setConstant(0);
99 // while(true) {
100 // aux_state = updateAuxState(); // Fill in this state from somewhere...
101 // end_effector->update(new_aux_state);
102 // end_effector->send();
103 // }
104 //
105 // Note -- you will probably want to check the return values of update and send to
106 // verify module connection is stable.
107 //
108 // Note that this is designed to be used with the arm API, but can also be used independently
109 // if desired.
110 template <size_t AuxSize>
111 class EffortEndEffector : public EndEffector<AuxSize> {
112 
113 public:
114  // Create a gripper group for n modules, using the modules' famil(ies) and name(s).
115  static std::unique_ptr<EffortEndEffector> create(const std::string& family, const std::string& name) {
116  return create(std::vector<std::string> { family }, std::vector<std::string> { name });
117  }
118 
119  // Create a gripper group for n modules, using the modules' famil(ies) and name(s).
120  static std::unique_ptr<EffortEndEffector> create(const std::vector<std::string>& families, const std::vector<std::string>& names) {
121  if (auto group = EndEffector<AuxSize>::getGroup(families, names))
122  return std::unique_ptr<EffortEndEffector>(new EffortEndEffector(group));
123  return nullptr;
124  }
125 
126  Eigen::VectorXd getState() {
127  return this->feedback_.getEfforts();
128  }
129 
130 protected:
131  void setCommand(size_t index, double value) override {
132  this->command_[index].actuator().effort().set(value);
133  }
134 
135 private:
136  EffortEndEffector(std::shared_ptr<hebi::Group> group) : EndEffector<AuxSize>(group) { }
137 };
138 
139 } // namespace arm
140 } // namespace experimental
141 } // namespace hebi
void setCommand(size_t index, double value) override
Definition: end_effector.hpp:131
static std::unique_ptr< EffortEndEffector > create(const std::vector< std::string > &families, const std::vector< std::string > &names)
Definition: end_effector.hpp:120
A list of Feedback objects that can be received from a Group of modules; the size() must match the nu...
Definition: group_feedback.hpp:18
bool loadGains(const std::string &gains_file)
Definition: end_effector.hpp:62
Definition: arm.cpp:8
static std::unique_ptr< EffortEndEffector > create(const std::string &family, const std::string &name)
Definition: end_effector.hpp:115
virtual void setCommand(size_t index, double value)=0
bool update(Eigen::VectorXd &aux_state) override
Definition: end_effector.hpp:34
virtual bool send()
Definition: end_effector.hpp:18
static std::shared_ptr< hebi::Group > getGroup(const std::vector< std::string > &families, const std::vector< std::string > &names)
Definition: end_effector.hpp:74
EndEffector(std::shared_ptr< hebi::Group > group)
Definition: end_effector.hpp:82
Definition: end_effector.hpp:111
hebi::GroupCommand command_
Definition: end_effector.hpp:84
A list of Command objects appropriate for sending to a Group of modules; the size() must match the nu...
Definition: group_command.hpp:19
bool send() override
Definition: end_effector.hpp:54
Maintains a registry of network-connected modules and returns Group objects to the user.
Definition: lookup.hpp:24
Definition: end_effector.hpp:12
virtual bool update(Eigen::VectorXd &aux_state)
Definition: end_effector.hpp:16
Eigen::VectorXd getState()
Definition: end_effector.hpp:126
Definition: end_effector.hpp:25
std::shared_ptr< Group > getGroupFromNames(const std::vector< std::string > &families, const std::vector< std::string > &names, int32_t timeout_ms=DEFAULT_TIMEOUT)
Get a group from modules with the given names and families.
Definition: lookup.cpp:27