HEBI C++ API  2.0.1
robot_model.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include "hebi.h"
4 
5 #include <memory>
6 #include <vector>
7 
8 #include "Eigen/Eigen"
9 #include "util.hpp"
10 
11 using namespace Eigen;
12 
13 namespace hebi {
14 namespace robot_model {
15 
16 using Matrix4dVector = std::vector<Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>;
17 using MatrixXdVector = std::vector<MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd>>;
18 // The result of an IK operation. More fields will be added to this structure
19 // in future API releases.
20 struct IKResult {
21  HebiStatusCode result; // Success or failure
22 };
23 
24 class RobotModel;
25 
26 class Objective {
27  friend RobotModel;
28 
29 public:
30  virtual ~Objective() {}
31 
32 protected:
33  virtual HebiStatusCode addObjective(HebiIKPtr ik) const = 0;
34 };
35 
37 public:
38  EndEffectorPositionObjective(const Eigen::Vector3d&);
39  EndEffectorPositionObjective(double weight, const Eigen::Vector3d&);
40 
41 private:
42  HebiStatusCode addObjective(HebiIKPtr ik) const override;
43  double _weight, _x, _y, _z;
44 };
45 
46 class EndEffectorSO3Objective final : public Objective {
47 public:
48  EndEffectorSO3Objective(const Eigen::Matrix3d&);
49  EndEffectorSO3Objective(double weight, const Eigen::Matrix3d&);
50 
51 private:
52  HebiStatusCode addObjective(HebiIKPtr ik) const override;
53  double _weight;
54  const double _matrix[9];
55 };
56 
57 class EndEffectorTipAxisObjective final : public Objective {
58 public:
59  EndEffectorTipAxisObjective(const Eigen::Vector3d&);
60  EndEffectorTipAxisObjective(double weight, const Eigen::Vector3d&);
61 
62 private:
63  HebiStatusCode addObjective(HebiIKPtr ik) const override;
64  double _weight, _x, _y, _z;
65 };
66 
67 class JointLimitConstraint final : public Objective {
68 public:
69  JointLimitConstraint(const Eigen::VectorXd& min_positions, const Eigen::VectorXd& max_positions);
70  JointLimitConstraint(double weight, const Eigen::VectorXd& min_positions, const Eigen::VectorXd& max_positions);
71 
72 private:
73  HebiStatusCode addObjective(HebiIKPtr ik) const override;
74  double _weight;
75  Eigen::VectorXd _min_positions;
76  Eigen::VectorXd _max_positions;
77 
78 public:
79  // Allow Eigen member variables:
80  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81 };
82 
91 template<size_t T>
92 inline void custom_objective_callback_wrapper(void* user_data, size_t num_positions, const double* positions,
93  double* errors);
94 
134 template<size_t N>
135 class CustomObjective final : public Objective {
136 public:
137  // This function is called with the following parameters:
138  // const std::vector<double>& positions
139  // std::array<double, N>& errors (NOTE: FILL THIS IN VIA THE CALLBACK FUNCTION)
140  using ObjectiveCallback = std::function<void(const std::vector<double>&, std::array<double, N>&)>;
141 
142  CustomObjective(ObjectiveCallback error_function) : _weight(1.0f), _callback(error_function) {}
143  CustomObjective(double weight, ObjectiveCallback error_function) : _weight(weight), _callback(error_function) {}
144 
145  // Note -- internal function to be called only from
146  // custom_objective_callback_wrapper.
147  void callCallback(void*, size_t num_positions, const double* positions, double* errors) const {
148  // Note -- the data here is copied to/from the C-style arrays. This isn't
149  // optimally efficient, but allows us to use type-checked C++ vectors and
150  // arrays. For performance critical applications, this could be modified
151  // to support user callbacks using the raw C-style arrays directly.
152 
153  // Process data into C++ structures.
154  std::vector<double> positions_array(num_positions);
155  for (size_t i = 0; i < num_positions; ++i)
156  positions_array[i] = positions[i];
157  // Note -- std::array is not guaranteed to be layout-compatible with a
158  // C-style array, even for POD, so we must copy here it we want the type
159  // safety of std::array.
160  std::array<double, N> errors_array;
161  for (size_t i = 0; i < N; ++i)
162  errors_array[i] = errors[i];
163 
164  _callback(positions_array, errors_array);
165 
166  for (size_t i = 0; i < N; ++i)
167  errors[i] = errors_array[i];
168  }
169 
170 private:
171  HebiStatusCode addObjective(HebiIKPtr ik) const override {
172  return hebiIKAddObjectiveCustom(ik, _weight, N, &custom_objective_callback_wrapper<N>,
173  const_cast<CustomObjective*>(this));
174  }
175  double _weight;
176  ObjectiveCallback _callback;
177 };
178 
184 template<size_t T>
185 inline void custom_objective_callback_wrapper(void* user_data, size_t num_positions, const double* positions,
186  double* errors) {
187  reinterpret_cast<CustomObjective<T>*>(user_data)->callCallback(user_data, num_positions, positions, errors);
188 }
189 
196 class RobotModel final {
197  friend Objective;
198 
199 private:
203  HebiRobotModelPtr const internal_;
204 
209  template<typename T>
210  HebiStatusCode addObjectives(HebiIKPtr ik, const T& objective) const {
211  static_assert(std::is_convertible<T*, Objective*>::value,
212  "Must pass arguments of base type hebi::robot_model::Objective to the variable args of solveIK!");
213  return static_cast<const Objective*>(&objective)->addObjective(ik);
214  }
215 
219  template<typename T, typename... Args>
220  HebiStatusCode addObjectives(HebiIKPtr ik, const T& objective, Args... args) const {
221  static_assert(std::is_convertible<T*, Objective*>::value,
222  "Must pass arguments of base type hebi::robot_model::Objective to the variable args of solveIK!");
223  auto res = static_cast<const Objective*>(&objective)->addObjective(ik);
224  if (res != HebiStatusSuccess)
225  return res;
226  return addObjectives(ik, args...);
227  }
228 
232  bool tryAdd(HebiRobotModelElementPtr element, bool combine);
233 
238  RobotModel(HebiRobotModelPtr);
239 
240 public:
241  enum class ActuatorType { X5_1, X5_4, X5_9, X8_3, X8_9, X8_16 };
242 
243  enum class LinkType { X5 };
244 
245  enum class BracketType {
246  X5LightLeft,
247  X5LightRight,
248  X5HeavyLeftInside,
249  X5HeavyLeftOutside,
250  X5HeavyRightInside,
251  X5HeavyRightOutside
252  };
253 
258  RobotModel();
259 
264  static std::unique_ptr<RobotModel> loadHRDF(const std::string& file);
265 
270  ~RobotModel() noexcept;
271 
283  void setBaseFrame(const Eigen::Matrix4d& base_frame);
284 
289  Eigen::Matrix4d getBaseFrame() const;
290 
300  size_t getFrameCount(HebiFrameType frame_type) const;
301 
306  size_t getDoFCount() const;
307 
330  bool addRigidBody(const Eigen::Matrix4d& com, const Eigen::VectorXd& inertia, double mass,
331  const Eigen::Matrix4d& output, bool combine);
332 
342  bool addJoint(HebiJointType joint_type, bool combine);
343 
350  bool addActuator(ActuatorType actuator_type);
351 
366  bool addLink(LinkType link_type, double extension, double twist);
367 
374  bool addBracket(BracketType bracket_type);
375 
381  void getForwardKinematics(HebiFrameType, const Eigen::VectorXd& positions, Matrix4dVector& frames) const;
407  void getFK(HebiFrameType, const Eigen::VectorXd& positions, Matrix4dVector& frames) const;
408 
428  void getEndEffector(const Eigen::VectorXd& positions, Eigen::Matrix4d& transform) const;
429 
436  template<typename... Args>
437  IKResult solveInverseKinematics(const Eigen::VectorXd& initial_positions, Eigen::VectorXd& result,
438  Args... args) const {
439  return solveIK(initial_positions, result, args...);
440  }
441 
456  template<typename... Args>
457  IKResult solveIK(const Eigen::VectorXd& initial_positions, Eigen::VectorXd& result, Args... objectives) const {
458  // Create a HEBI C library IK object
459  auto ik = hebiIKCreate();
460 
461  // (Try) to add objectives to the IK object
462  IKResult res;
463  res.result = addObjectives(ik, objectives...);
464  if (res.result != HebiStatusSuccess) {
465  hebiIKRelease(ik);
466  return res;
467  }
468 
469  // Transfer/initialize from Eigen to C arrays
470  auto positions_array = new double[initial_positions.size()];
471  {
472  Map<Eigen::VectorXd> tmp(positions_array, initial_positions.size());
473  tmp = initial_positions;
474  }
475  auto result_array = new double[initial_positions.size()];
476 
477  // Call into C library to solve
478  res.result = hebiIKSolve(ik, internal_, positions_array, result_array, nullptr);
479 
480  // Transfer/cleanup from C arrays to Eigen
481  delete[] positions_array;
482  {
483  Map<Eigen::VectorXd> tmp(result_array, initial_positions.size());
484  result = tmp;
485  }
486  delete[] result_array;
487 
488  hebiIKRelease(ik);
489 
490  return res;
491  }
492 
498  void getJacobians(HebiFrameType, const Eigen::VectorXd& positions, MatrixXdVector& jacobians) const;
512  void getJ(HebiFrameType, const Eigen::VectorXd& positions, MatrixXdVector& jacobians) const;
513 
519  void getJacobianEndEffector(const Eigen::VectorXd& positions, Eigen::MatrixXd& jacobian) const;
539  void getJEndEffector(const Eigen::VectorXd& positions, Eigen::MatrixXd& jacobian) const;
540 
549  void getMasses(Eigen::VectorXd& masses) const;
550 
551 private:
556 };
557 
558 } // namespace robot_model
559 } // namespace hebi
std::vector< Matrix4d, Eigen::aligned_allocator< Eigen::Matrix4d > > Matrix4dVector
Definition: robot_model.hpp:16
BracketType
Definition: robot_model.hpp:245
void custom_objective_callback_wrapper(void *user_data, size_t num_positions, const double *positions, double *errors)
C-style callback wrapper to call into CustomObjective class; this should only be used by the CustomOb...
Definition: robot_model.hpp:185
Definition: color.hpp:5
CustomObjective(ObjectiveCallback error_function)
Definition: robot_model.hpp:142
#define HEBI_DISABLE_COPY_MOVE(Class)
Definition: util.hpp:8
Represents a chain or tree of robot elements (rigid bodies and joints).
Definition: robot_model.hpp:196
IKResult solveIK(const Eigen::VectorXd &initial_positions, Eigen::VectorXd &result, Args... objectives) const
Solves for an inverse kinematics solution given a set of objectives.
Definition: robot_model.hpp:457
HebiStatusCode result
Definition: robot_model.hpp:21
Definition: robot_model.hpp:67
virtual ~Objective()
Definition: robot_model.hpp:30
LinkType
Definition: robot_model.hpp:243
Definition: robot_model.hpp:26
Definition: robot_model.hpp:46
Allows you to add a custom objective function.
Definition: robot_model.hpp:135
void callCallback(void *, size_t num_positions, const double *positions, double *errors) const
Definition: robot_model.hpp:147
Definition: robot_model.hpp:20
std::function< void(const std::vector< double > &, std::array< double, N > &)> ObjectiveCallback
Definition: robot_model.hpp:140
ActuatorType
Definition: robot_model.hpp:241
std::vector< MatrixXd, Eigen::aligned_allocator< Eigen::MatrixXd > > MatrixXdVector
Definition: robot_model.hpp:17
CustomObjective(double weight, ObjectiveCallback error_function)
Definition: robot_model.hpp:143