19 #ifndef OP3_KINEMATICS_DYNAMICS_H_ 20 #define OP3_KINEMATICS_DYNAMICS_H_ 23 #include <eigen3/Eigen/Eigen> 47 std::vector<int>
findRoute(
int from,
int to);
50 Eigen::MatrixXd
calcMC(
int joint_id);
51 Eigen::MatrixXd
calcCOM(Eigen::MatrixXd mc);
57 Eigen::MatrixXd
calcVWerr(Eigen::MatrixXd tar_position, Eigen::MatrixXd curr_position,
58 Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation);
60 bool calcInverseKinematics(
int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation,
int max_iter,
62 bool calcInverseKinematics(
int from,
int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation,
63 int max_iter,
double ik_err);
66 bool calcInverseKinematics(
int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation,
int max_iter,
67 double ik_err, Eigen::MatrixXd weight);
68 bool calcInverseKinematics(
int from,
int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation,
69 int max_iter,
double ik_err, Eigen::MatrixXd weight);
81 Eigen::MatrixXd
getJointAxis(
const std::string link_name);
87 Eigen::MatrixXd K, Eigen::MatrixXd P);
Eigen::MatrixXd calcJacobianCOM(std::vector< int > idx)
LinkData * op3_link_data_[ALL_JOINT_ID+1]
void calcForwardKinematics(int joint_ID)
Eigen::MatrixXd calcJacobian(std::vector< int > idx)
std::vector< int > findRoute(int to)
bool calcInverseKinematicsForLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
double leg_side_offset_m_
double getJointDirection(const std::string link_name)
Eigen::MatrixXd getJointAxis(const std::string link_name)
Eigen::MatrixXd calcPreviewParam(double preview_time, double control_cycle, double lipm_height, Eigen::MatrixXd K, Eigen::MatrixXd P)
Eigen::MatrixXd calcMC(int joint_id)
bool calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)
Eigen::MatrixXd calcCOM(Eigen::MatrixXd mc)
LinkData * getLinkData(const std::string link_name)
double calcTotalMass(int joint_id)
bool calcInverseKinematicsForRightLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
bool calcInverseKinematicsForLeftLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
Eigen::MatrixXd calcVWerr(Eigen::MatrixXd tar_position, Eigen::MatrixXd curr_position, Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation)