109 lines
3.1 KiB
C
109 lines
3.1 KiB
C
|
//
|
||
|
|
// Created by xtkuang on 2025/7/7.
|
||
|
|
//
|
||
|
|
|
||
|
|
#ifndef JOINT_H
|
||
|
|
#define JOINT_H
|
||
|
|
|
||
|
|
#include <eigen3/Eigen/Core>
|
||
|
|
#include <memory>
|
||
|
|
#include "utils/math/constants.h"
|
||
|
|
#include "utils/math/liegroup.h"
|
||
|
|
#include "utils/dynamics/link.h"
|
||
|
|
|
||
|
|
namespace cmvr::dyn {
|
||
|
|
class Joint : public std::enable_shared_from_this<Joint> {
|
||
|
|
public:
|
||
|
|
template<int DOF>
|
||
|
|
friend class Robot;
|
||
|
|
|
||
|
|
static std::shared_ptr<Joint> Make(std::string name, math::se3v::MatrixType S);
|
||
|
|
|
||
|
|
static std::shared_ptr<Joint> MakeRevoluteJoint(std::string name,
|
||
|
|
const math::SE3::MatrixType &T = math::SE3::Identity(),
|
||
|
|
const Eigen::Vector3d &axis = {0, 0, 1});
|
||
|
|
|
||
|
|
static std::shared_ptr<Joint> MakePrismaticJoint(std::string name,
|
||
|
|
const math::SE3::MatrixType &T = math::SE3::Identity(),
|
||
|
|
const Eigen::Vector3d &axis = {0, 0, 1});
|
||
|
|
|
||
|
|
static std::shared_ptr<Joint> MakeFixedJoint(std::string name);
|
||
|
|
|
||
|
|
std::string GetName() const;
|
||
|
|
|
||
|
|
void ConnectLinks(const std::shared_ptr<Link> &parent_link, const std::shared_ptr<Link> &child_link,
|
||
|
|
const math::SE3::MatrixType &T_pj = math::SE3::Identity(),
|
||
|
|
const math::SE3::MatrixType &T_jc = math::SE3::Identity());
|
||
|
|
|
||
|
|
void Disconnect();
|
||
|
|
|
||
|
|
void SetLimitQ(double lower, double upper);
|
||
|
|
|
||
|
|
void SetLimitQdot(double lower, double upper);
|
||
|
|
|
||
|
|
void SetLimitQddot(double lower, double upper);
|
||
|
|
|
||
|
|
void SetLimitTorque(double value);
|
||
|
|
|
||
|
|
double GetLimitQLower() const;
|
||
|
|
|
||
|
|
double GetLimitQUpper() const;
|
||
|
|
|
||
|
|
double GetLimitQdotLower() const;
|
||
|
|
|
||
|
|
double GetLimitQdotUpper() const;
|
||
|
|
|
||
|
|
double GetLimitQddotLower() const;
|
||
|
|
|
||
|
|
double GetLimitQddotUpper() const;
|
||
|
|
|
||
|
|
double GetLimitTorque() const;
|
||
|
|
|
||
|
|
void SetLimitQLower(double val);
|
||
|
|
|
||
|
|
void SetLimitQUpper(double val);
|
||
|
|
|
||
|
|
void SetLimitQdotLower(double val);
|
||
|
|
|
||
|
|
void SetLimitQdotUpper(double val);
|
||
|
|
|
||
|
|
void SetLimitQddotLower(double val);
|
||
|
|
|
||
|
|
void SetLimitQddotUpper(double val);
|
||
|
|
|
||
|
|
std::weak_ptr<Link> GetParentLink();
|
||
|
|
|
||
|
|
std::shared_ptr<Link> GetChildLink();
|
||
|
|
|
||
|
|
std::shared_ptr<const Link> GetChildLink() const;
|
||
|
|
|
||
|
|
bool IsFixed() const;
|
||
|
|
|
||
|
|
private:
|
||
|
|
explicit Joint(std::string name);
|
||
|
|
|
||
|
|
Joint(std::string name, math::se3v::MatrixType S);
|
||
|
|
|
||
|
|
private:
|
||
|
|
std::string name_{};
|
||
|
|
bool fixed_;
|
||
|
|
math::se3v::MatrixType S_;
|
||
|
|
double limit_torque_{(std::numeric_limits<double>::max)()};
|
||
|
|
double limit_q_lower_{-(std::numeric_limits<double>::max)()};
|
||
|
|
double limit_q_upper_{(std::numeric_limits<double>::max)()};
|
||
|
|
double limit_qdot_lower_{-(std::numeric_limits<double>::max)()};
|
||
|
|
double limit_qdot_upper_{(std::numeric_limits<double>::max)()};
|
||
|
|
// double limit_qddot_lower_{-(std::numeric_limits<double>::max)()};
|
||
|
|
// double limit_qddot_upper_{(std::numeric_limits<double>::max)()};
|
||
|
|
double limit_qddot_lower_{-10.};
|
||
|
|
double limit_qddot_upper_{10.}; // (rad/s^2)
|
||
|
|
|
||
|
|
std::weak_ptr<Link> parent_link_;
|
||
|
|
std::shared_ptr<Link> child_link_{nullptr};
|
||
|
|
math::SE3::MatrixType T_pj_, T_jc_;
|
||
|
|
};
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
#endif //JOINT_H
|