6 #ifndef FLEXIV_RDK_MODEL_HPP_
7 #define FLEXIV_RDK_MODEL_HPP_
10 #include <Eigen/Eigen>
33 const Eigen::Vector3d& gravity_vector = Eigen::Vector3d(0.0, 0.0, -9.81));
60 void Update(
const std::vector<double>& positions,
const std::vector<double>& velocities);
73 Eigen::MatrixXd
J(
const std::string& link_name);
87 Eigen::MatrixXd
dJ(
const std::string& link_name);
138 void SyncURDF(
const std::string& template_urdf_path);
151 std::pair<bool, std::vector<double>>
reachable(
const std::array<double, kPoseSize>& pose,
152 const std::vector<double>& seed_positions,
bool free_orientation)
const;
169 std::unique_ptr<Impl> pimpl_;
Interface to obtain certain model data of the robot, including kinematics and dynamics.
Eigen::MatrixXd J(const std::string &link_name)
[Non-blocking] Compute and get the Jacobian matrix at the frame of the specified link ,...
Model(const Robot &robot, const Eigen::Vector3d &gravity_vector=Eigen::Vector3d(0.0, 0.0, -9.81))
[Non-blocking] Instantiate the robot model interface.
Eigen::VectorXd c()
[Non-blocking] Compute and get the Coriolis force vector for the generalized coordinates,...
Eigen::MatrixXd dJ(const std::string &link_name)
[Non-blocking] Compute and get the time derivative of Jacobian matrix at the frame of the specified l...
void Reload()
[Blocking] Reload (refresh) parameters of the robot model stored locally in this class using the late...
std::pair< bool, std::vector< double > > reachable(const std::array< double, kPoseSize > &pose, const std::vector< double > &seed_positions, bool free_orientation) const
[Blocking] Check if a Cartesian pose is reachable. If yes, also return an IK solution of the correspo...
void Update(const std::vector< double > &positions, const std::vector< double > &velocities)
[Non-blocking] Update the configuration (posture) of the locally-stored robot model so that the dynam...
std::pair< double, double > configuration_score() const
[Blocking] Score of the robot's current configuration (posture), calculated from the manipulability m...
Eigen::VectorXd g()
[Non-blocking] Compute and get the gravity force vector for the generalized coordinates,...
Eigen::MatrixXd M()
[Non-blocking] Compute and get the mass matrix for the generalized coordinates, i....
void SyncURDF(const std::string &template_urdf_path)
[Blocking] Sync the actual kinematic parameters of the connected robot into the template URDF.
Eigen::MatrixXd C()
[Non-blocking] Compute and get the Coriolis/centripetal matrix for the generalized coordinates,...
Main interface to control the robot, containing several function categories and background services.