HEBI C++ API  3.9.0
arm.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include <set>
4 
5 // HEBI C++ API components
6 #include "group.hpp"
7 #include "group_command.hpp"
8 #include "group_feedback.hpp"
9 #include "robot_config.hpp"
10 #include "robot_model.hpp"
11 #include "trajectory.hpp"
12 
13 // Arm API components
14 #include "end_effector.hpp"
15 #include "goal.hpp"
16 #include "kinematics_helper.hpp"
17 
18 namespace hebi {
19 namespace experimental {
20 namespace arm {
21 
22 // Forward declare for usage in plugin.
23 class Arm;
24 
25 namespace plugin {
26 
43 class Plugin {
44 public:
45  Plugin(const std::string& name) : name_(name) {}
46  virtual ~Plugin() = default;
47 
48  // The name of this plugin
49  std::string name() const { return name_; }
50 
51  // Determines if the plugin's effects are enabled. Note that every
52  // plugin is always invoked by Arm::update, and the plugin itself is
53  // responsible for using this state (and the current ramped "enabled ratio")
54  // to moderate its effect.
55  bool enabled() const { return enabled_; }
56  // Sets enabled state. If "ramp time" > 0, the "enabled" state is immediately
57  // set to the new value, but "enabled_ratio" will change gradually.
58  void setEnabled(bool enabled) { enabled_ = enabled; }
59  // Float value between 0 and 1 of "how enabled" we are as we are
60  // ramping between disabled/enabled state.
61  float enabledRatio() { return enabled_ratio_; }
62  // Set how fast we ramp between enabled and disabled states; returns "false"
63  // if value given is invalid.
64  bool setRampTime(float ramp_time);
65  // How fast we ramp between enabled and disabled states
66  float rampTime() { return ramp_time_; }
67 
68  // Callback which updates state on the arm. Invoked by Arm::update.
69  // Returns `true` on success and `false` otherwise.
70  bool update(Arm&, double dt);
71 
72  // Can be overridden by plugin implementations if necessary; called by `Arm::send`
73  virtual bool send() { return true; }
74 
75  // Override to update any state based on the associated arm.
76  // Invoked when the instance is added to an arm via `Arm::addPlugin`. Returns
77  // "false" if there was an error when attempting to add.
78  virtual bool onAssociated(const Arm&) { return true; }
79 
80 protected:
81  // When the object is created through the "create" factory method,
82  // parameters should be applied in turn. Each parameter should be
83  // validated, and an error returned if they cannot be applied.
84  //
85  // Implementations should override the "applyParameterImpl" functions for parameters of types
86  // they have. The main "applyParameters" function is called to iterate through the config
87  // structure, and should be called by each implementing class' "create" method
88  bool applyParameters(const PluginConfig& config, std::set<std::string> required_parameters);
89  virtual bool applyParameterImpl(const std::string& name, bool value) { return false; }
90  virtual bool applyParameterImpl(const std::string& name, const std::vector<bool>& value) { return false; }
91  virtual bool applyParameterImpl(const std::string& name, float value) { return false; }
92  virtual bool applyParameterImpl(const std::string& name, const std::vector<float>& value) { return false; }
93  virtual bool applyParameterImpl(const std::string& name, const std::string& value) { return false; }
94  virtual bool applyParameterImpl(const std::string& name, const std::vector<std::string>& value) { return false; }
95 
96  // Overridden by plugin implementations, and called by `Plugin::update`
97  virtual bool updateImpl(Arm&, double dt) = 0;
98 
99 private:
100  virtual bool applyParameter(const std::string& name, bool value); // we implement for "enabled"
101  virtual bool applyParameter(const std::string& name, const std::vector<bool>& value) {
102  return applyParameterImpl(name, value);
103  }
104  virtual bool applyParameter(const std::string& name, float value); // we implement for "ramp_time"
105  virtual bool applyParameter(const std::string& name, const std::vector<float>& value) {
106  return applyParameterImpl(name, value);
107  }
108  virtual bool applyParameter(const std::string& name, const std::string& value) {
109  return applyParameterImpl(name, value);
110  }
111  virtual bool applyParameter(const std::string& name, const std::vector<std::string>& value) {
112  return applyParameterImpl(name, value);
113  }
114 
115  // Name of the plugin
116  const std::string name_{};
117  // How long it takes to fully transition enabled state.
118  float ramp_time_{};
119  // Whether the plugin is enabled
120  bool enabled_{true};
121  // Current linear level between off (0) and on (1); updated during `update`
122  float enabled_ratio_{1.f};
123 };
124 
125 using Factory = std::function<std::unique_ptr<Plugin>(const PluginConfig&)>;
126 
138 public:
139  static std::unique_ptr<GravityCompensationEffort> create(const PluginConfig&);
140  static std::string pluginTypeName() { return "GravityCompensationEffort"; };
141  bool onAssociated(const Arm& arm) override;
142 
143 protected:
144  // For "imu_feedback_index" and "imu_frame_index"
145  bool applyParameterImpl(const std::string& name, float value) override;
146  // For "imu_rotation_offset"
147  bool applyParameterImpl(const std::string& name, const std::vector<float>& value) override;
148  bool updateImpl(Arm& arm, double dt) override;
149 
150 private:
151  GravityCompensationEffort(const std::string& name) : Plugin(name) {}
152  // Cached helper var
153  Eigen::VectorXd grav_efforts_;
154  // Parameters
155  size_t imu_feedback_index_{};
156  size_t imu_frame_index_{};
157  Eigen::Matrix3d imu_local_transform_{Eigen::Matrix3d::Identity()};
158 };
159 
160 class ImpedanceController : public Plugin {
161 public:
162  static std::unique_ptr<ImpedanceController> create(const PluginConfig&);
163  static std::string pluginTypeName() { return "ImpedanceController"; };
164  bool onAssociated(const Arm& arm) override;
165 
166 protected:
167  // For "gains_in_end_effector_frame"
168  bool applyParameterImpl(const std::string& name, bool value) override;
169  // For "kp", "kd", "ki", and "i_clamp"
170  bool applyParameterImpl(const std::string& name, const std::vector<float>& value) override;
171  bool updateImpl(Arm& arm, double dt) override;
172 
173 private:
174  ImpedanceController(const std::string& name) : Plugin(name) {}
175 
176  // Cached helper vars
177 
178  // Current integral error term
179  Eigen::VectorXd i_error_{Eigen::VectorXd::Zero(6)};
180 
181  // Parameters
182 
183  // Translations and Rotations can be specified in the
184  // base frame or in the end effector frame.
185  bool gains_in_end_effector_frame_{};
186  // Impendance Control Gains
187  // NOTE: The gains correspond to:
188  // [ trans_x trans_y trans_z rot_x rot_y rot_z ]
189  Eigen::VectorXd kp_{Eigen::VectorXd::Zero(6)}; // (N/m) or (Nm/rad)
190  Eigen::VectorXd kd_{Eigen::VectorXd::Zero(6)}; // (N/(m/sec)) or (Nm/(rad/sec))
191  Eigen::VectorXd ki_{Eigen::VectorXd::Zero(6)}; // (N/(m*sec)) or (Nm/(rad*sec))
192  Eigen::VectorXd i_clamp_;
193 };
194 
195 class EffortOffset : public Plugin {
196 public:
197  static std::unique_ptr<EffortOffset> create(const PluginConfig&);
198  static std::string pluginTypeName() { return "EffortOffset"; };
199  bool onAssociated(const Arm& arm) override;
200 
201 protected:
202  // For "offset"
203  bool applyParameterImpl(const std::string& name, const std::vector<float>& value) override;
204  bool updateImpl(Arm& arm, double dt) override;
205 
206 private:
207  EffortOffset(const std::string& name) : Plugin(name) {}
208  // Parameters
209  Eigen::VectorXd effort_offsets_{};
210 };
211 
212 } // namespace plugin
213 
214 static std::map<std::string, plugin::Factory> ArmPluginMap = {
218 };
219 
220 // A high-level abstraction of a robot arm, coordinating kinematics, control,
221 // and basic motion planning.
222 //
223 // Typical usage is as follows; the robot.cfg file includes information such as
224 // module family and names, HRDFs, gains, etc.
225 //
226 // std::vector<std::string> errors;
227 // auto cfg = RobotConfig::loadConfig("robot.cfg", errors);
228 // if (!cfg)
229 // return; // see contents of "errors"
230 // auto arm = Arm::create(*cfg);
231 // if (!arm)
232 // return; // are modules on network?
233 //
234 // arm->loadGains(cfg->getGains("default"));
235 //
236 // while(true) {
237 // arm->update();
238 // arm->send();
239 // if (some_condition)
240 // arm->setGoal(target_goal);
241 // }
242 //
243 // (Note -- in an actual application, you would want to verify the return
244 // values of many of the functions above to ensure proper operation!)
245 class Arm {
246 public:
248  // Setup functions
250 
251  // Parameters for creating an arm
252  struct Params {
253  // The family and names passed to the "lookup" function to find modules
254  // Both are required.
255  std::vector<std::string> families_;
256  std::vector<std::string> names_;
257  // How long a command takes effect for on the robot before expiring.
258  int command_lifetime_ = 100;
259  // Loop rate, in Hz. This is how fast the arm update loop will nominally
260  // run.
261  double control_frequency_ = 200.f;
262 
263  // The robot description. Either supply the hrdf_file _or_ the robot_model.
264  std::string hrdf_file_;
265  std::shared_ptr<robot_model::RobotModel> robot_model_;
266 
267  // Optionally, supply an end effector to be controlled by the "aux" state of
268  // provided goals.
269  std::shared_ptr<EndEffectorBase> end_effector_;
270 
271  // A function pointer that returns a double representing the current time in
272  // seconds. (Can be overloaded to use, e.g., simulator time)
273  //
274  // The default value uses the steady clock wall time.
275  std::function<double()> get_current_time_s_ = []() {
276  using clock = std::chrono::steady_clock;
277  static const clock::time_point start_time = clock::now();
278  return (std::chrono::duration<double>(clock::now() - start_time)).count();
279  };
280  };
281 
282  // Creates an "Arm" object; uses the RobotConfig file for information about the robot.
283  static std::unique_ptr<Arm> create(const RobotConfig& config);
284 
285  // Creates an "Arm" object, and puts it into a "weightless" no-goal control
286  // mode.
287  static std::unique_ptr<Arm> create(const Params& params);
288 
289  // Adds the plugin to the arm object, taking ownership of the plugin.
290  bool addPlugin(std::unique_ptr<plugin::Plugin> plugin);
291 
292  // Returns a weak pointer to the first plugin found of the given type, or nullopt
293  // if nothing is found.
294  template<class T>
295  std::weak_ptr<plugin::Plugin> getPluginByType() {
296  for (auto& p : plugins_) {
297  if (dynamic_cast<T*>(p.get()) != nullptr)
298  return p;
299  }
300  return {};
301  }
302 
303  std::weak_ptr<plugin::Plugin> getPluginByName(const std::string& name);
304 
305  // Loads gains from the given .xml file, and sends them to the module. Returns
306  // false if the gains file could not be found, if these is a mismatch in
307  // number of modules, or the modules do not acknowledge receipt of the gains.
308  bool loadGains(const std::string& gains_file);
309 
311  // Accessors
313 
314  // Returns the number of modules / DoF in the arm
315  size_t size() const { return group_->size(); }
316 
317  // Returns the internal group. Not necessary for most use cases.
318  const Group& group() const { return *group_; }
319 
320  // Returns the internal robot model. Not necessary for most use cases.
321  const robot_model::RobotModel& robotModel() const { return *robot_model_; }
322  // Returns the (non-const) internal robot model. Not necessary for most use cases.
323  // Use with care, as modifying the properties of the underlying robot model while
324  // using the arm may result in undefined behavior.
325  robot_model::RobotModel& robotModel() { return *robot_model_; }
326 
327  // Returns the currently active internal trajectory. Not necessary for most
328  // use cases.
329  // Returns 'nullptr' if there is no active trajectory.
330  const trajectory::Trajectory* trajectory() const { return trajectory_.get(); }
331 
332  // Returns the end effector object, if given. Not necessary for most use
333  // cases.
334  // Returns 'nullptr' if there is no end effector.
335  const EndEffectorBase* endEffector() const { return end_effector_.get(); }
336 
337  // Returns the command last computed by update, or an empty command object
338  // if "update" has never successfully run. The returned command can be
339  // modified as desired before it is sent to the robot with the send function.
340  GroupCommand& pendingCommand() { return command_; }
341  const GroupCommand& pendingCommand() const { return command_; }
342 
343  // Returns the last feedback obtained by update, or an empty feedback object
344  // if "update" has never successfully run.
345  const GroupFeedback& lastFeedback() const { return feedback_; }
346 
348  // Main loop functions
349  //
350  // Typical usage:
351  //
352  // while(true) {
353  // arm->update();
354  // arm->send();
355  // }
357 
358  // Updates feedback and generates the basic command for this timestep.
359  // To retrieve the feedback, call `getLastFeedback()` after this call.
360  // You can modify the command object after calling this.
361  //
362  // Returns 'false' on a connection problem; true on success.
363  bool update();
364 
365  // Sends the command last computed by "update" to the robot arm. Any user
366  // modifications to the command are included.
367  bool send();
368 
370  // Goals
371  //
372  // A goal is a desired (joint angle) position that the arm should reach, and
373  // optionally information about the time it should reach that goal at and the
374  // path (position, velocity, and acceleration waypoints) it should take to
375  // get there.
376  //
377  // The default behavior when a goal is set is for the arm to plan and begin
378  // executing a smooth motion from its current state to this goal, with an
379  // internal heuristic that defines the time at which it will reach the goal.
380  // This immediately overrides any previous goal that was set.
381  //
382  // If there is no "active" goal the arm is set into a mode where it is
383  // actively controlled to be approximately weightless, and can be moved around
384  // by hand easily. This is the default state when the arm is created.
385  //
386  // After reaching the goal, the arm continues to be commanded with the final
387  // joint state of the set goal, and is _not_ implicitly returned to a
388  // "weightless" mode.
389  //
390  // A goal may also define "aux" states to be sent to an end effector
391  // associated with the arm. In this case, the end effector states are
392  // treated as "step functions", immediately being commanded at the timestamp
393  // of the waypoint they are associated with. An empty "aux" goal or "NaN"
394  // defines a "no transition" at the given waypoint.
396 
397  // Set the current goal waypoint(s), immediately replanning to these
398  // location(s) and optionally end effector states.
399  // Goal is a commanded position / velocity.
400  void setGoal(const Goal& goal);
401 
402  // Set the state of aux, if added (e.g., end effector). Overrides any
403  // future aux waypoints.
404  template<typename T>
405  void setAuxState(const T& aux_state) {
406  auto aux_size = aux_state.size();
407  if (aux_state.size() > 0) {
408  aux_times_.resize(1);
409  aux_times_[0] = get_current_time_s_();
410  aux_.resize(aux_size, 1);
411  // Replaces 'aux_.leftCols(1) = aux_state;'
412  for (size_t i = 0; i < aux_size; i++) {
413  aux_(i, 0) = aux_state[i];
414  }
415  } else {
416  // Reset aux states
417  aux_.resize(0, 0);
418  aux_times_.resize(0);
419  }
420  }
421 
422  // Returns the progress (from 0 to 1) of the current goal, per the last
423  // update call.
424  //
425  // If we have reached the goal, progress is "1". If there is no active goal,
426  // or we have just begun, progress is "0".
427  double goalProgress() const;
428 
429  // Have we reached the goal? If there is no goal, returns 'false'
430  bool atGoal() const { return goalProgress() >= 1.0; }
431 
432  // Cancels any active goal, returning to a "weightless" state which does not
433  // actively command position or velocity.
434  void cancelGoal();
435 
437  // Helper functions for forward and inverse kinematics.
439 
440  // Use the following joint limits when computing IK
441  // Affects future calls to solveIK**.
442  void setJointLimits(const Eigen::VectorXd& min_positions, const Eigen::VectorXd& max_positions) {
443  kinematics_helper_.setJointLimits(*robot_model_, min_positions, max_positions);
444  }
445 
446  // Do not use joint limits when computing IK
447  // Affects future calls to solveIK**.
448  void clearJointLimits(const Eigen::VectorXd& min_positions, const Eigen::VectorXd& max_positions) {
449  kinematics_helper_.clearJointLimits();
450  }
451 
452  // Get the end effector (x,y,z) location
453  Eigen::Vector3d FK(const Eigen::VectorXd& positions) const {
454  return kinematics_helper_.FK3Dof(*robot_model_, positions);
455  }
456 
457  // Get the end effector (x,y,z) location and direction, represented by a unit-length vector
458  void FK(const Eigen::VectorXd& positions, Eigen::Vector3d& xyz_out, Eigen::Vector3d& tip_axis) const {
459  kinematics_helper_.FK5Dof(*robot_model_, positions, xyz_out, tip_axis);
460  }
461 
462  // Get the end effector (x,y,z) location and orientation (represented by a rotation matrix)
463  void FK(const Eigen::VectorXd& positions, Eigen::Vector3d& xyz_out, Eigen::Matrix3d& orientation) const {
464  kinematics_helper_.FK6Dof(*robot_model_, positions, xyz_out, orientation);
465  }
466 
467  // Return the joint angles to move to a given xyz location
468  Eigen::VectorXd solveIK(const Eigen::VectorXd& initial_positions, const Eigen::Vector3d& target_xyz) const {
469  return kinematics_helper_.solveIK3Dof(*robot_model_, initial_positions, target_xyz);
470  }
471 
472  // Return the joint angles to move to a given xyz location while
473  // pointing a certain direction
474  Eigen::VectorXd solveIK(const Eigen::VectorXd& initial_positions, const Eigen::Vector3d& target_xyz,
475  const Eigen::Vector3d& end_tip) const {
476  return kinematics_helper_.solveIK5Dof(*robot_model_, initial_positions, target_xyz, end_tip);
477  }
478 
479  // Return the joint angles to move to a given xyz location while
480  // pointing a certain direction
481  Eigen::VectorXd solveIK(const Eigen::VectorXd& initial_positions, const Eigen::Vector3d& target_xyz,
482  const Eigen::Matrix3d& orientation) const {
483  return kinematics_helper_.solveIK6Dof(*robot_model_, initial_positions, target_xyz, orientation);
484  }
485 
486 private:
487  // Private arm constructor
488  Arm(std::function<double()> get_current_time_s, std::shared_ptr<Group> group,
489  std::shared_ptr<robot_model::RobotModel> robot_model, std::shared_ptr<EndEffectorBase> end_effector = nullptr)
490  : get_current_time_s_(get_current_time_s),
491  last_time_(get_current_time_s()),
492  group_(std::move(group)),
493  robot_model_(std::move(robot_model)),
494  end_effector_(std::move(end_effector)),
495  pos_(Eigen::VectorXd::Zero(group_->size())),
496  vel_(Eigen::VectorXd::Zero(group_->size())),
497  accel_(Eigen::VectorXd::Zero(group_->size())),
498  feedback_(group_->size()),
499  command_(group_->size()) {}
500 
501  // Returns the aux state at this point in the trajectory
502  Eigen::VectorXd getAux(double t) const;
503 
504  std::function<double()> get_current_time_s_;
505  double last_time_;
506  std::shared_ptr<Group> group_;
507  std::shared_ptr<robot_model::RobotModel> robot_model_;
508  std::shared_ptr<EndEffectorBase> end_effector_;
509 
510  // The joint angle trajectory for reaching the current goal.
511  std::shared_ptr<trajectory::Trajectory> trajectory_;
512  double trajectory_start_time_{std::numeric_limits<double>::quiet_NaN()};
513  // These are just temporary variables to cache output from
514  // Trajectory::getState.
515  Eigen::VectorXd pos_;
516  Eigen::VectorXd vel_;
517  Eigen::VectorXd accel_;
518 
519  // Along with a trajectory, aux states may be set. These are the last aux
520  // state for each timestep in the trajectory:
521  Eigen::VectorXd aux_times_;
522  Eigen::MatrixXd aux_;
523 
524  // Robot model helpers for FK + IK
525  internal::KinematicsHelper kinematics_helper_;
526 
527  hebi::GroupFeedback feedback_;
528  hebi::GroupCommand command_;
529 
530  // Current arm plugins
531  std::vector<std::shared_ptr<plugin::Plugin>> plugins_;
532 };
533 
534 } // namespace arm
535 } // namespace experimental
536 } // namespace hebi
virtual bool send()
Definition: arm.hpp:73
size_t size() const
Definition: arm.hpp:315
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
Eigen::VectorXd solveIK6Dof(const robot_model::RobotModel &robot_model, const Eigen::VectorXd &initial_positions, const Eigen::Vector3d &target_xyz, const Eigen::Matrix3d &orientation) const
Definition: kinematics_helper.cpp:98
std::string name() const
Definition: arm.hpp:49
const trajectory::Trajectory * trajectory() const
Definition: arm.hpp:330
Represents a group of physical HEBI modules, and allows Command, Feedback, and Info objects to be sen...
Definition: group.hpp:35
std::shared_ptr< robot_model::RobotModel > robot_model_
Definition: arm.hpp:265
Plugin(const std::string &name)
Definition: arm.hpp:45
const robot_model::RobotModel & robotModel() const
Definition: arm.hpp:321
void cancelGoal()
Definition: arm.cpp:626
void FK6Dof(const robot_model::RobotModel &robot_model, const Eigen::VectorXd &positions, Eigen::Vector3d &xyz_out, Eigen::Matrix3d &orientation) const
Definition: kinematics_helper.cpp:147
bool updateImpl(Arm &arm, double dt) override
Definition: arm.cpp:221
void clearJointLimits()
Definition: kinematics_helper.cpp:38
Eigen::VectorXd solveIK(const Eigen::VectorXd &initial_positions, const Eigen::Vector3d &target_xyz, const Eigen::Matrix3d &orientation) const
Definition: arm.hpp:481
bool updateImpl(Arm &arm, double dt) override
Definition: arm.cpp:319
Eigen::VectorXd solveIK5Dof(const robot_model::RobotModel &robot_model, const Eigen::VectorXd &initial_positions, const Eigen::Vector3d &target_xyz, const Eigen::Vector3d &end_tip) const
Definition: kinematics_helper.cpp:70
static std::unique_ptr< GravityCompensationEffort > create(const PluginConfig &)
Definition: arm.cpp:101
void FK5Dof(const robot_model::RobotModel &robot_model, const Eigen::VectorXd &positions, Eigen::Vector3d &xyz_out, Eigen::Vector3d &tip_axis) const
Definition: kinematics_helper.cpp:135
void clearJointLimits(const Eigen::VectorXd &min_positions, const Eigen::VectorXd &max_positions)
Definition: arm.hpp:448
bool applyParameterImpl(const std::string &name, const std::vector< float > &value) override
Definition: arm.cpp:308
static std::string pluginTypeName()
Definition: arm.hpp:198
Eigen::VectorXd solveIK(const Eigen::VectorXd &initial_positions, const Eigen::Vector3d &target_xyz, const Eigen::Vector3d &end_tip) const
Definition: arm.hpp:474
virtual bool applyParameterImpl(const std::string &name, const std::string &value)
Definition: arm.hpp:93
bool enabled() const
Definition: arm.hpp:55
Definition: arm.cpp:8
void FK(const Eigen::VectorXd &positions, Eigen::Vector3d &xyz_out, Eigen::Vector3d &tip_axis) const
Definition: arm.hpp:458
Eigen::VectorXd solveIK3Dof(const robot_model::RobotModel &robot_model, const Eigen::VectorXd &initial_positions, const Eigen::Vector3d &target_xyz) const
Definition: kinematics_helper.cpp:45
double control_frequency_
Definition: arm.hpp:261
bool setRampTime(float ramp_time)
Definition: arm.cpp:14
const GroupFeedback & lastFeedback() const
Definition: arm.hpp:345
bool send()
Definition: arm.cpp:535
Represents a chain or tree of robot elements (rigid bodies and joints).
Definition: robot_model.hpp:412
bool addPlugin(std::unique_ptr< plugin::Plugin > plugin)
Definition: arm.cpp:456
virtual bool applyParameterImpl(const std::string &name, const std::vector< float > &value)
Definition: arm.hpp:92
std::string hrdf_file_
Definition: arm.hpp:264
bool atGoal() const
Definition: arm.hpp:430
Eigen::Vector3d FK3Dof(const robot_model::RobotModel &robot_model, const Eigen::VectorXd &positions) const
Definition: kinematics_helper.cpp:126
std::function< std::unique_ptr< Plugin >(const PluginConfig &)> Factory
Definition: arm.hpp:125
static std::map< std::string, plugin::Factory > ArmPluginMap
Definition: arm.hpp:214
virtual bool applyParameterImpl(const std::string &name, const std::vector< std::string > &value)
Definition: arm.hpp:94
static std::unique_ptr< ImpedanceController > create(const PluginConfig &)
Definition: arm.cpp:181
void setJointLimits(const Eigen::VectorXd &min_positions, const Eigen::VectorXd &max_positions)
Definition: arm.hpp:442
const Group & group() const
Definition: arm.hpp:318
bool onAssociated(const Arm &arm) override
Definition: arm.cpp:301
bool applyParameterImpl(const std::string &name, float value) override
Definition: arm.cpp:118
static std::string pluginTypeName()
Definition: arm.hpp:163
bool applyParameters(const PluginConfig &config, std::set< std::string > required_parameters)
Definition: arm.cpp:21
const EndEffectorBase * endEffector() const
Definition: arm.hpp:335
static std::unique_ptr< EffortOffset > create(const PluginConfig &)
Definition: arm.cpp:294
void FK(const Eigen::VectorXd &positions, Eigen::Vector3d &xyz_out, Eigen::Matrix3d &orientation) const
Definition: arm.hpp:463
int command_lifetime_
Definition: arm.hpp:258
void setEnabled(bool enabled)
Definition: arm.hpp:58
const GroupCommand & pendingCommand() const
Definition: arm.hpp:341
double goalProgress() const
Definition: arm.cpp:616
Definition: goal.hpp:16
virtual bool onAssociated(const Arm &)
Definition: arm.hpp:78
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 onAssociated(const Arm &arm) override
Definition: arm.cpp:108
bool update(Arm &, double dt)
Definition: arm.cpp:86
std::weak_ptr< plugin::Plugin > getPluginByType()
Definition: arm.hpp:295
float enabledRatio()
Definition: arm.hpp:61
bool onAssociated(const Arm &arm) override
Definition: arm.cpp:188
virtual bool applyParameterImpl(const std::string &name, float value)
Definition: arm.hpp:91
void setAuxState(const T &aux_state)
Definition: arm.hpp:405
std::function< double()> get_current_time_s_
Definition: arm.hpp:275
Definition: plugin_config.hpp:12
Definition: end_effector.hpp:12
bool loadGains(const std::string &gains_file)
Definition: arm.cpp:477
Eigen::VectorXd solveIK(const Eigen::VectorXd &initial_positions, const Eigen::Vector3d &target_xyz) const
Definition: arm.hpp:468
Represents a smooth trajectory through a set of waypoints.
Definition: trajectory.hpp:19
robot_model::RobotModel & robotModel()
Definition: arm.hpp:325
static std::unique_ptr< Arm > create(const RobotConfig &config)
Definition: arm.cpp:326
std::weak_ptr< plugin::Plugin > getPluginByName(const std::string &name)
Definition: arm.cpp:469
GroupCommand & pendingCommand()
Definition: arm.hpp:340
Definition: robot_config.hpp:13
void setGoal(const Goal &goal)
Definition: arm.cpp:552
Definition: arm.hpp:245
Eigen::Vector3d FK(const Eigen::VectorXd &positions) const
Definition: arm.hpp:453
std::vector< std::string > names_
Definition: arm.hpp:256
virtual bool applyParameterImpl(const std::string &name, const std::vector< bool > &value)
Definition: arm.hpp:90
bool applyParameterImpl(const std::string &name, bool value) override
Definition: arm.cpp:192
virtual bool updateImpl(Arm &, double dt)=0
std::vector< std::string > families_
Definition: arm.hpp:255
Plugin for providing gravity compensating torques for the arm.
Definition: arm.hpp:137
static std::string pluginTypeName()
Definition: arm.hpp:140
float rampTime()
Definition: arm.hpp:66
virtual bool applyParameterImpl(const std::string &name, bool value)
Definition: arm.hpp:89
bool updateImpl(Arm &arm, double dt) override
Definition: arm.cpp:155
void setJointLimits(const robot_model::RobotModel &robot_model, const Eigen::VectorXd &min_positions, const Eigen::VectorXd &max_positions)
Definition: kinematics_helper.cpp:8
std::shared_ptr< EndEffectorBase > end_effector_
Definition: arm.hpp:269
bool update()
Definition: arm.cpp:485