11 using namespace Eigen;
14 namespace robot_model {
16 using Matrix4dVector = std::vector<Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>;
17 using MatrixXdVector = std::vector<MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd>>;
33 virtual HebiStatusCode addObjective(HebiIKPtr ik)
const = 0;
42 HebiStatusCode addObjective(HebiIKPtr ik)
const override;
43 double _weight, _x, _y, _z;
52 HebiStatusCode addObjective(HebiIKPtr ik)
const override;
54 const double _matrix[9];
63 HebiStatusCode addObjective(HebiIKPtr ik)
const override;
64 double _weight, _x, _y, _z;
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);
73 HebiStatusCode addObjective(HebiIKPtr ik)
const override;
75 Eigen::VectorXd _min_positions;
76 Eigen::VectorXd _max_positions;
80 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
140 using ObjectiveCallback = std::function<void(
const std::vector<double>&, std::array<double, N>&)>;
147 void callCallback(
void*,
size_t num_positions,
const double* positions,
double* errors)
const {
154 std::vector<double> positions_array(num_positions);
155 for (
size_t i = 0; i < num_positions; ++i)
156 positions_array[i] = positions[i];
160 std::array<double, N> errors_array;
161 for (
size_t i = 0; i < N; ++i)
162 errors_array[i] = errors[i];
164 _callback(positions_array, errors_array);
166 for (
size_t i = 0; i < N; ++i)
167 errors[i] = errors_array[i];
171 HebiStatusCode addObjective(HebiIKPtr ik)
const override {
172 return hebiIKAddObjectiveCustom(ik, _weight, N, &custom_objective_callback_wrapper<N>,
173 const_cast<CustomObjective*>(
this));
176 ObjectiveCallback _callback;
187 reinterpret_cast<CustomObjective<T>*
>(user_data)->callCallback(user_data, num_positions, positions, errors);
190 class ActuatorMetadata;
191 class BracketMetadata;
194 class RigidBodyMetadata;
197 Other = HebiRobotModelElementTypeOther,
198 Actuator = HebiRobotModelElementTypeActuator,
199 Bracket = HebiRobotModelElementTypeBracket,
200 Joint = HebiRobotModelElementTypeJoint,
201 Link = HebiRobotModelElementTypeLink,
202 RigidBody = HebiRobotModelElementTypeRigidBody,
208 Output = HebiFrameTypeOutput,
210 Input = HebiFrameTypeInput
223 X5_1 = HebiActuatorTypeX5_1,
224 X5_4 = HebiActuatorTypeX5_4,
225 X5_9 = HebiActuatorTypeX5_9,
226 X8_3 = HebiActuatorTypeX8_3,
227 X8_9 = HebiActuatorTypeX8_9,
228 X8_16 = HebiActuatorTypeX8_16,
229 R8_3 = HebiActuatorTypeR8_3,
230 R8_9 = HebiActuatorTypeR8_9,
231 R8_16 = HebiActuatorTypeR8_16
241 Inline = HebiLinkInputTypeInline
246 Inline = HebiLinkOutputTypeInline
265 Custom = HebiEndEffectorTypeCustom,
284 return static_cast<ElementType>(metadata_.element_type_);
293 if (elementType() == ElementType::Actuator) {
294 return reinterpret_cast<const ActuatorMetadata*>(
this);
305 if (elementType() == ElementType::Bracket) {
306 return reinterpret_cast<const BracketMetadata*>(
this);
317 if (elementType() == ElementType::Joint) {
318 return reinterpret_cast<const JointMetadata*>(
this);
329 if (elementType() == ElementType::Link) {
330 return reinterpret_cast<const LinkMetadata*>(
this);
341 if (elementType() == ElementType::RigidBody) {
342 return reinterpret_cast<const RigidBodyMetadata*>(
this);
349 const HebiRobotModelElementMetadata&
metadata()
const {
return metadata_; }
351 HebiRobotModelElementMetadata metadata_;
400 float extension()
const {
return metadata().extension_; }
405 float twist()
const {
return metadata().twist_; }
426 HebiRobotModelPtr
const internal_;
433 HebiStatusCode addObjectives(HebiIKPtr ik,
const T& objective)
const {
434 static_assert(std::is_convertible<T*, Objective*>::value,
435 "Must pass arguments of base type hebi::robot_model::Objective to the variable args of solveIK!");
436 return static_cast<const Objective*>(&objective)->addObjective(ik);
442 template<
typename T,
typename... Args>
443 HebiStatusCode addObjectives(HebiIKPtr ik,
const T& objective, Args... args)
const {
444 static_assert(std::is_convertible<T*, Objective*>::value,
445 "Must pass arguments of base type hebi::robot_model::Objective to the variable args of solveIK!");
446 auto res = static_cast<const Objective*>(&objective)->addObjective(ik);
447 if (res != HebiStatusSuccess)
449 return addObjectives(ik, args...);
455 bool tryAdd(HebiRobotModelElementPtr element);
478 static std::unique_ptr<RobotModel> loadHRDF(
const std::string& file);
484 static std::unique_ptr<RobotModel> loadHRDFString(
const std::string&
string);
503 void setBaseFrame(const Eigen::Matrix4d& base_frame);
509 Eigen::Matrix4d getBaseFrame() const;
520 size_t getFrameCount(
FrameType frame_type) const;
528 size_t getFrameCount(HebiFrameType frame_type)
const {
return getFrameCount(static_cast<FrameType>(frame_type)); }
534 size_t getDoFCount()
const;
549 bool addRigidBody(
const Eigen::Matrix4d& com,
const Eigen::VectorXd& inertia,
double mass,
550 const Eigen::Matrix4d& output);
568 [[deprecated (
"Replaced by hebi::robot_model::RobotModel::addJoint(hebi::robot_model::JointType)")]]
569 bool addJoint(HebiJointType joint_type) {
return addJoint(static_cast<JointType>(joint_type)); }
624 bool addEndEffector(
double x,
double y,
double z);
638 bool addEndEffector(
const Eigen::Matrix4d& com,
const Eigen::VectorXd& inertia,
double mass,
639 const Eigen::Matrix4d& output);
652 [[deprecated (
"Change first argument form HebiFrameType to hebi::robot_model::FrameType")]]
654 getForwardKinematics(static_cast<FrameType>(frame_type), positions, frames);
689 [[deprecated (
"Change first argument form HebiFrameType to hebi::robot_model::FrameType")]]
691 getFK(static_cast<FrameType>(frame_type), positions, frames);
713 void getEndEffector(
const Eigen::VectorXd& positions, Eigen::Matrix4d& transform)
const;
721 template<
typename... Args>
723 Args... args)
const {
724 return solveIK(initial_positions, result, args...);
741 template<
typename... Args>
742 IKResult solveIK(
const Eigen::VectorXd& initial_positions, Eigen::VectorXd& result, Args... objectives)
const {
744 auto ik = hebiIKCreate();
748 res.
result = addObjectives(ik, objectives...);
749 if (res.
result != HebiStatusSuccess) {
755 auto positions_array =
new double[initial_positions.size()];
757 Map<Eigen::VectorXd> tmp(positions_array, initial_positions.size());
758 tmp = initial_positions;
760 auto result_array =
new double[initial_positions.size()];
763 res.
result = hebiIKSolve(ik, internal_, positions_array, result_array,
nullptr);
766 delete[] positions_array;
768 Map<Eigen::VectorXd> tmp(result_array, initial_positions.size());
771 delete[] result_array;
792 getJacobians(static_cast<FrameType>(frame_type), positions, jacobians);
815 [[deprecated (
"Change first argument form HebiFrameType to hebi::robot_model::FrameType")]]
816 void getJ(HebiFrameType frame_type,
const Eigen::VectorXd& positions,
MatrixXdVector& jacobians)
const {
817 getJ(static_cast<FrameType>(frame_type), positions, jacobians);
825 void getJacobianEndEffector(
const Eigen::VectorXd& positions, Eigen::MatrixXd& jacobian)
const;
845 void getJEndEffector(
const Eigen::VectorXd& positions, Eigen::MatrixXd& jacobian)
const;
855 void getMasses(Eigen::VectorXd& masses)
const;
860 void getMetadata(std::vector<MetadataBase>& metadata)
const;
ActuatorType
Definition: robot_model.hpp:222
IKResult solveInverseKinematics(const Eigen::VectorXd &initial_positions, Eigen::VectorXd &result, Args... args) const
Solves for an inverse kinematics solution given a set of objectives.
Definition: robot_model.hpp:722
std::vector< Matrix4d, Eigen::aligned_allocator< Eigen::Matrix4d > > Matrix4dVector
Definition: robot_model.hpp:16
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
JointType
Definition: robot_model.hpp:213
LinkType
Definition: robot_model.hpp:234
BracketType
Definition: robot_model.hpp:249
CustomObjective(ObjectiveCallback error_function)
Definition: robot_model.hpp:142
#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:791
Represents a chain or tree of robot elements (rigid bodies and joints).
Definition: robot_model.hpp:419
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:742
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:816
Definition: robot_model.hpp:36
HebiStatusCode result
Definition: robot_model.hpp:21
EndEffectorType
Definition: robot_model.hpp:264
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:690
virtual ~Objective()
Definition: robot_model.hpp:30
Definition: robot_model.hpp:57
Definition: robot_model.hpp:26
size_t getFrameCount(HebiFrameType frame_type) const
Return the number of frames in the forward kinematics.
Definition: robot_model.hpp:528
Definition: robot_model.hpp:46
Rigid Body specific view of an element in a RobotModel instance.
Definition: robot_model.hpp:411
Allows you to add a custom objective function.
Definition: robot_model.hpp:135
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:653
void callCallback(void *, size_t num_positions, const double *positions, double *errors) const
Definition: robot_model.hpp:147
FrameType
Definition: robot_model.hpp:206
LinkOutputType
Definition: robot_model.hpp:244
Definition: robot_model.hpp:20
std::function< void(const std::vector< double > &, std::array< double, N > &)> ObjectiveCallback
Definition: robot_model.hpp:140
ElementType
Definition: robot_model.hpp:196
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:569
CustomObjective(double weight, ObjectiveCallback error_function)
Definition: robot_model.hpp:143