HEBI C++ API  3.11.1
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
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(CustomObjective&) = delete;
143  CustomObjective(CustomObjective&&) = delete;
144  CustomObjective(ObjectiveCallback error_function) : _weight(1.0f), _callback(error_function) {}
145  CustomObjective(double weight, ObjectiveCallback error_function) : _weight(weight), _callback(error_function) {}
146 
147  // Note -- internal function to be called only from
148  // custom_objective_callback_wrapper.
149  void callCallback(void*, size_t num_positions, const double* positions, double* errors) const {
150  // Note -- the data here is copied to/from the C-style arrays. This isn't
151  // optimally efficient, but allows us to use type-checked C++ vectors and
152  // arrays. For performance critical applications, this could be modified
153  // to support user callbacks using the raw C-style arrays directly.
154 
155  // Process data into C++ structures.
156  std::vector<double> positions_array(num_positions);
157  for (size_t i = 0; i < num_positions; ++i)
158  positions_array[i] = positions[i];
159  // Note -- std::array is not guaranteed to be layout-compatible with a
160  // C-style array, even for POD, so we must copy here it we want the type
161  // safety of std::array.
162  std::array<double, N> errors_array;
163  for (size_t i = 0; i < N; ++i)
164  errors_array[i] = errors[i];
165 
166  _callback(positions_array, errors_array);
167 
168  for (size_t i = 0; i < N; ++i)
169  errors[i] = errors_array[i];
170  }
171 
172 private:
173  HebiStatusCode addObjective(HebiIKPtr ik) const override {
174  return hebiIKAddObjectiveCustom(ik, _weight, N, &custom_objective_callback_wrapper<N>,
175  const_cast<CustomObjective*>(this));
176  }
177  double _weight;
178  ObjectiveCallback _callback;
179 };
180 
186 template<size_t T>
187 inline void custom_objective_callback_wrapper(void* user_data, size_t num_positions, const double* positions,
188  double* errors) {
189  reinterpret_cast<CustomObjective<T>*>(user_data)->callCallback(user_data, num_positions, positions, errors);
190 }
191 
192 class ActuatorMetadata;
193 class BracketMetadata;
194 class JointMetadata;
195 class LinkMetadata;
196 class RigidBodyMetadata;
197 
198 enum class ElementType {
199  Other = HebiRobotModelElementTypeOther,
200  Actuator = HebiRobotModelElementTypeActuator,
201  Bracket = HebiRobotModelElementTypeBracket,
202  Joint = HebiRobotModelElementTypeJoint,
203  Link = HebiRobotModelElementTypeLink,
204  RigidBody = HebiRobotModelElementTypeRigidBody,
205  EndEffector = HebiRobotModelElementTypeEndEffector
206 };
207 
208 enum class FrameType {
209  CenterOfMass = HebiFrameTypeCenterOfMass,
210  Output = HebiFrameTypeOutput,
211  EndEffector = HebiFrameTypeEndEffector,
212  Input = HebiFrameTypeInput,
213  Mesh = HebiFrameTypeMesh
214 };
215 
216 enum class JointType {
217  RotationX = HebiJointTypeRotationX,
218  RotationY = HebiJointTypeRotationY,
219  RotationZ = HebiJointTypeRotationZ,
220  TranslationX = HebiJointTypeTranslationX,
221  TranslationY = HebiJointTypeTranslationY,
222  TranslationZ = HebiJointTypeTranslationZ
223 };
224 
225 enum class ActuatorType {
226  X5_1 = HebiActuatorTypeX5_1,
227  X5_4 = HebiActuatorTypeX5_4,
228  X5_9 = HebiActuatorTypeX5_9,
229  X8_3 = HebiActuatorTypeX8_3,
230  X8_9 = HebiActuatorTypeX8_9,
231  X8_16 = HebiActuatorTypeX8_16,
232  R8_3 = HebiActuatorTypeR8_3,
233  R8_9 = HebiActuatorTypeR8_9,
234  R8_16 = HebiActuatorTypeR8_16
235 };
236 
237 enum class LinkType { X5 = HebiLinkTypeX5, X8 = HebiLinkTypeR8 };
238 
239 enum class LinkInputType { RightAngle = HebiLinkInputTypeRightAngle, Inline = HebiLinkInputTypeInline };
240 
241 enum class LinkOutputType { RightAngle = HebiLinkOutputTypeRightAngle, Inline = HebiLinkOutputTypeInline };
242 
243 enum class BracketType {
244  X5LightLeft = HebiBracketTypeX5LightLeft,
245  X5LightRight = HebiBracketTypeX5LightRight,
246  X5HeavyLeftInside = HebiBracketTypeX5HeavyLeftInside,
247  X5HeavyLeftOutside = HebiBracketTypeX5HeavyLeftOutside,
248  X5HeavyRightInside = HebiBracketTypeX5HeavyRightInside,
249  X5HeavyRightOutside = HebiBracketTypeX5HeavyRightOutside,
250  R8LightLeft = HebiBracketTypeR8LightLeft,
251  R8LightRight = HebiBracketTypeR8LightRight,
252  R8HeavyLeftInside = HebiBracketTypeR8HeavyLeftInside,
253  R8HeavyLeftOutside = HebiBracketTypeR8HeavyLeftOutside,
254  R8HeavyRightInside = HebiBracketTypeR8HeavyRightInside,
255  R8HeavyRightOutside = HebiBracketTypeR8HeavyRightOutside
256 };
257 
258 enum class EndEffectorType {
259  Custom = HebiEndEffectorTypeCustom,
260  X5Parallel = HebiEndEffectorTypeX5Parallel,
261  R8Parallel = HebiEndEffectorTypeR8Parallel
262 };
263 
268  friend class RobotModel;
269 
270 public:
271  MetadataBase() = default;
272  MetadataBase(const MetadataBase&) = delete;
273  MetadataBase& operator=(const MetadataBase&) = delete;
274  MetadataBase(MetadataBase&&) = default;
275  MetadataBase& operator=(MetadataBase&&) = default;
276 
277  ElementType elementType() const { return static_cast<ElementType>(metadata_.element_type_); }
278 
284  const ActuatorMetadata* asActuator() const {
285  if (elementType() == ElementType::Actuator) {
286  return reinterpret_cast<const ActuatorMetadata*>(this);
287  }
288  return nullptr;
289  }
290 
296  const BracketMetadata* asBracket() const {
297  if (elementType() == ElementType::Bracket) {
298  return reinterpret_cast<const BracketMetadata*>(this);
299  }
300  return nullptr;
301  }
302 
308  const JointMetadata* asJoint() const {
309  if (elementType() == ElementType::Joint) {
310  return reinterpret_cast<const JointMetadata*>(this);
311  }
312  return nullptr;
313  }
314 
320  const LinkMetadata* asLink() const {
321  if (elementType() == ElementType::Link) {
322  return reinterpret_cast<const LinkMetadata*>(this);
323  }
324  return nullptr;
325  }
326 
333  if (elementType() == ElementType::RigidBody) {
334  return reinterpret_cast<const RigidBodyMetadata*>(this);
335  }
336  return nullptr;
337  }
338 
339 protected:
340  // Raw data can only be accessed by the sub classes
341  const HebiRobotModelElementMetadata& metadata() const { return metadata_; }
342 
343 private:
344  HebiRobotModelElementMetadata metadata_;
345 };
346 
351 public:
355  ActuatorType actuatorType() const { return static_cast<ActuatorType>(metadata().actuator_type_); }
356 };
357 
362 public:
366  BracketType bracketType() const { return static_cast<BracketType>(metadata().bracket_type_); }
367 };
368 
372 class JointMetadata : public MetadataBase {
373 public:
377  JointType jointType() const { return static_cast<JointType>(metadata().joint_type_); }
378 };
379 
383 class LinkMetadata : public MetadataBase {
384 public:
388  LinkType linkType() const { return static_cast<LinkType>(metadata().link_type_); }
389 
393  float extension() const { return metadata().extension_; }
394 
398  float twist() const { return metadata().twist_; }
399 };
400 
404 class RigidBodyMetadata : public MetadataBase {};
405 
412 class RobotModel final {
413  friend Objective;
414 
415 private:
419  HebiRobotModelPtr const internal_;
420 
425  template<typename T>
426  HebiStatusCode addObjectives(HebiIKPtr ik, const T& objective) const {
427  static_assert(std::is_convertible<T*, Objective*>::value,
428  "Must pass arguments of base type hebi::robot_model::Objective to the variable args of solveIK!");
429  return static_cast<const Objective*>(&objective)->addObjective(ik);
430  }
431 
435  template<typename T, typename... Args>
436  HebiStatusCode addObjectives(HebiIKPtr ik, const T& objective, const Args&... args) const {
437  static_assert(std::is_convertible<T*, Objective*>::value,
438  "Must pass arguments of base type hebi::robot_model::Objective to the variable args of solveIK!");
439  auto res = static_cast<const Objective*>(&objective)->addObjective(ik);
440  if (res != HebiStatusSuccess)
441  return res;
442  return addObjectives(ik, args...);
443  }
444 
448  bool tryAdd(HebiRobotModelElementPtr element);
449 
454  RobotModel(HebiRobotModelPtr);
455 
456 public:
457  using ActuatorType [[deprecated("Replace with hebi::robot_model::ActuatorType")]] = robot_model::ActuatorType;
458  using BracketType [[deprecated("Replace with hebi::robot_model::BracketType")]] = robot_model::BracketType;
459  using LinkType [[deprecated("Replace with hebi::robot_model::LinkType")]] = robot_model::LinkType;
460 
465  RobotModel();
466 
470  static std::unique_ptr<RobotModel> loadHRDF(const std::string& file);
471 
476  static std::unique_ptr<RobotModel> loadHRDFString(const std::string& string);
477 
494  std::unique_ptr<RobotModel> createSubtree(size_t element_index);
495 
500  ~RobotModel() noexcept;
501 
513  void setBaseFrame(const Eigen::Matrix4d& base_frame);
514 
519  Eigen::Matrix4d getBaseFrame() const;
520 
530  size_t getFrameCount(FrameType frame_type) const;
531 
537  [[deprecated("Replaced by hebi::robot_model::RobotModel::getFrameCount(hebi::robot_model::FrameType)")]] size_t
538  getFrameCount(HebiFrameType frame_type) const {
539  return getFrameCount(static_cast<FrameType>(frame_type));
540  }
541 
546  size_t getDoFCount() const;
547 
557  std::string getMeshPath(size_t mesh_frame_index) const;
558 
572  bool addRigidBody(const Eigen::Matrix4d& com, const Eigen::VectorXd& inertia, double mass,
573  const Eigen::Matrix4d& output);
574 
584  bool addJoint(JointType joint_type);
585 
591  [[deprecated("Replaced by hebi::robot_model::RobotModel::addJoint(hebi::robot_model::JointType)")]] bool addJoint(
592  HebiJointType joint_type) {
593  return addJoint(static_cast<JointType>(joint_type));
594  }
595 
602  bool addActuator(robot_model::ActuatorType actuator_type);
603 
620  bool addLink(robot_model::LinkType link_type, double extension, double twist,
621  LinkInputType input_type = LinkInputType::RightAngle,
622  LinkOutputType output_type = LinkOutputType::RightAngle);
623 
630  bool addBracket(robot_model::BracketType bracket_type);
631 
640  bool addEndEffector(EndEffectorType end_effector_type);
649  bool addEndEffector(double x, double y, double z);
663  bool addEndEffector(const Eigen::Matrix4d& com, const Eigen::VectorXd& inertia, double mass,
664  const Eigen::Matrix4d& output);
670  void getForwardKinematics(FrameType, const Eigen::VectorXd& positions, Matrix4dVector& frames) const;
671 
677  [[deprecated("Change first argument form HebiFrameType to hebi::robot_model::FrameType")]] void getForwardKinematics(
678  HebiFrameType frame_type, const Eigen::VectorXd& positions, Matrix4dVector& frames) const {
679  getForwardKinematics(static_cast<FrameType>(frame_type), positions, frames);
680  }
681 
707  void getFK(FrameType, const Eigen::VectorXd& positions, Matrix4dVector& frames) const;
708 
714  [[deprecated("Change first argument form HebiFrameType to hebi::robot_model::FrameType")]] void getFK(
715  HebiFrameType frame_type, const Eigen::VectorXd& positions, Matrix4dVector& frames) const {
716  getFK(static_cast<FrameType>(frame_type), positions, frames);
717  }
718 
738  void getEndEffector(const Eigen::VectorXd& positions, Eigen::Matrix4d& transform) const;
739 
746  template<typename... Args>
747  IKResult solveInverseKinematics(const Eigen::VectorXd& initial_positions, Eigen::VectorXd& result,
748  const Args&... args) const {
749  return solveIK(initial_positions, result, args...);
750  }
751 
766  template<typename... Args>
767  IKResult solveIK(const Eigen::VectorXd& initial_positions, Eigen::VectorXd& result, const Args&... objectives) const {
768  // Create a HEBI C library IK object
769  auto ik = hebiIKCreate();
770 
771  // (Try) to add objectives to the IK object
772  IKResult res;
773  res.result = addObjectives(ik, objectives...);
774  if (res.result != HebiStatusSuccess) {
775  hebiIKRelease(ik);
776  return res;
777  }
778 
779  // Transfer/initialize from Eigen to C arrays
780  auto positions_array = new double[initial_positions.size()];
781  {
782  Map<Eigen::VectorXd> tmp(positions_array, initial_positions.size());
783  tmp = initial_positions;
784  }
785  auto result_array = new double[initial_positions.size()];
786 
787  // Call into C library to solve
788  res.result = hebiIKSolve(ik, internal_, positions_array, result_array, nullptr);
789 
790  // Transfer/cleanup from C arrays to Eigen
791  delete[] positions_array;
792  {
793  Map<Eigen::VectorXd> tmp(result_array, initial_positions.size());
794  result = tmp;
795  }
796  delete[] result_array;
797 
798  hebiIKRelease(ik);
799 
800  return res;
801  }
802 
808  void getJacobians(FrameType, const Eigen::VectorXd& positions, MatrixXdVector& jacobians) const;
809 
815  [[deprecated]] void getJacobians(HebiFrameType frame_type, const Eigen::VectorXd& positions,
816  MatrixXdVector& jacobians) const {
817  getJacobians(static_cast<FrameType>(frame_type), positions, jacobians);
818  }
819 
833  void getJ(FrameType, const Eigen::VectorXd& positions, MatrixXdVector& jacobians) const;
834 
840  [[deprecated("Change first argument form HebiFrameType to hebi::robot_model::FrameType")]] void getJ(
841  HebiFrameType frame_type, const Eigen::VectorXd& positions, MatrixXdVector& jacobians) const {
842  getJ(static_cast<FrameType>(frame_type), positions, jacobians);
843  }
844 
850  void getJacobianEndEffector(const Eigen::VectorXd& positions, Eigen::MatrixXd& jacobian) const;
870  void getJEndEffector(const Eigen::VectorXd& positions, Eigen::MatrixXd& jacobian) const;
871 
880  void getMasses(Eigen::VectorXd& masses) const;
881 
885  void getMetadata(std::vector<MetadataBase>& metadata) const;
886 
887  /*
888  * \param comp_torque A vector which is filled with the torques necessary
889  * to offset the effect of gravity on this robot model. This vector is
890  * resized as necessary inside this function (it is set to have length
891  * equal to the number of degrees of freedom).
892  */
893  void getGravCompEfforts(const Eigen::VectorXd& angles, const Eigen::Vector3d& gravity,
894  Eigen::VectorXd& comp_torque) const;
895 
896  /*
897  * Computes the torques necessary to accelerate the masses within the arm, given the
898  * current configuration.
899  */
900  void getDynamicCompEfforts(const Eigen::VectorXd& angles, const Eigen::VectorXd& cmd_pos, const Eigen::VectorXd& cmd_vel,
901  const Eigen::VectorXd& cmd_accel, Eigen::VectorXd& comp_torque, double dt) const;
902 
903 private:
908 };
909 
910 } // namespace robot_model
911 } // namespace hebi
ActuatorType
Definition: robot_model.hpp:225
std::vector< Matrix4d, Eigen::aligned_allocator< Eigen::Matrix4d > > Matrix4dVector
Definition: robot_model.hpp:16
const RigidBodyMetadata * asRigidBody() const
View the rigid body specific fields, if this metadata describes a rigid body.
Definition: robot_model.hpp:332
Link specific view of an element in a RobotModel instance.
Definition: robot_model.hpp:383
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:187
const ActuatorMetadata * asActuator() const
View the actuator specific fields, if this metadata describes an actuator.
Definition: robot_model.hpp:284
JointType
Definition: robot_model.hpp:216
JointType jointType() const
Specifies the particular joint type.
Definition: robot_model.hpp:377
LinkType
Definition: robot_model.hpp:237
Definition: arm.cpp:10
const BracketMetadata * asBracket() const
View the bracket specific fields, if this metadata describes a bracket.
Definition: robot_model.hpp:296
BracketType
Definition: robot_model.hpp:243
CustomObjective(ObjectiveCallback error_function)
Definition: robot_model.hpp:144
#define HEBI_DISABLE_COPY_MOVE(Class)
Definition: util.hpp:6
void getJacobians(HebiFrameType frame_type, const Eigen::VectorXd &positions, MatrixXdVector &jacobians) const
Generates the Jacobian for each frame in the given kinematic tree.
Definition: robot_model.hpp:815
ActuatorType actuatorType() const
Specifies the particular actuator model.
Definition: robot_model.hpp:355
Represents a chain or tree of robot elements (rigid bodies and joints).
Definition: robot_model.hpp:412
void getJ(HebiFrameType frame_type, const Eigen::VectorXd &positions, MatrixXdVector &jacobians) const
Generates the Jacobian for each frame in the given kinematic tree.
Definition: robot_model.hpp:840
float twist() const
The twist of the link (e.g., the value specified in invocation of RobotModel::addLink)
Definition: robot_model.hpp:398
Actuator specific view of an element in a RobotModel instance.
Definition: robot_model.hpp:350
IKResult solveIK(const Eigen::VectorXd &initial_positions, Eigen::VectorXd &result, const Args &... objectives) const
Solves for an inverse kinematics solution given a set of objectives.
Definition: robot_model.hpp:767
ElementType elementType() const
Definition: robot_model.hpp:277
static Eigen::VectorXd getGravCompEfforts(const hebi::robot_model::RobotModel &model, const Eigen::VectorXd &, const hebi::GroupFeedback &feedback)
Definition: grav_comp.hpp:36
const JointMetadata * asJoint() const
View the joint specific fields, if this metadata describes a joint.
Definition: robot_model.hpp:308
HebiStatusCode result
Definition: robot_model.hpp:21
EndEffectorType
Definition: robot_model.hpp:258
Definition: robot_model.hpp:67
LinkInputType
Definition: robot_model.hpp:239
void getFK(HebiFrameType frame_type, const Eigen::VectorXd &positions, Matrix4dVector &frames) const
Generates the forward kinematics for the given robot model.
Definition: robot_model.hpp:714
IKResult solveInverseKinematics(const Eigen::VectorXd &initial_positions, Eigen::VectorXd &result, const Args &... args) const
Solves for an inverse kinematics solution given a set of objectives.
Definition: robot_model.hpp:747
Base class for all metadata. Do not instantiate directly.
Definition: robot_model.hpp:267
virtual ~Objective()
Definition: robot_model.hpp:30
Definition: robot_model.hpp:26
Definition: robot_model.hpp:46
Rigid Body specific view of an element in a RobotModel instance.
Definition: robot_model.hpp:404
BracketType bracketType() const
Specifies the particular bracket type.
Definition: robot_model.hpp:366
Allows you to add a custom objective function.
Definition: robot_model.hpp:135
Bracket specific view of an element in a RobotModel instance.
Definition: robot_model.hpp:361
void getForwardKinematics(HebiFrameType frame_type, const Eigen::VectorXd &positions, Matrix4dVector &frames) const
Generates the forward kinematics for the given robot model.
Definition: robot_model.hpp:677
Joint specific view of an element in a RobotModel instance.
Definition: robot_model.hpp:372
void callCallback(void *, size_t num_positions, const double *positions, double *errors) const
Definition: robot_model.hpp:149
FrameType
Definition: robot_model.hpp:208
LinkOutputType
Definition: robot_model.hpp:241
Definition: robot_model.hpp:20
std::function< void(const std::vector< double > &, std::array< double, N > &)> ObjectiveCallback
Definition: robot_model.hpp:140
float extension() const
The extension of the link (e.g., the value specified in invocation of RobotModel::addLink)
Definition: robot_model.hpp:393
ElementType
Definition: robot_model.hpp:198
LinkType linkType() const
Specifies the particular link type.
Definition: robot_model.hpp:388
const HebiRobotModelElementMetadata & metadata() const
Definition: robot_model.hpp:341
std::vector< MatrixXd, Eigen::aligned_allocator< Eigen::MatrixXd > > MatrixXdVector
Definition: robot_model.hpp:17
bool addJoint(HebiJointType joint_type)
Adds a degree of freedom about the specified axis.
Definition: robot_model.hpp:591
CustomObjective(double weight, ObjectiveCallback error_function)
Definition: robot_model.hpp:145
const LinkMetadata * asLink() const
View the link specific fields, if this metadata describes a link.
Definition: robot_model.hpp:320