#include <op3_kinematics_dynamics.h>
Public Member Functions | |
| Eigen::MatrixXd | calcCOM (Eigen::MatrixXd mc) |
| void | calcForwardKinematics (int joint_ID) |
| bool | calcInverseKinematics (int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err) |
| bool | calcInverseKinematics (int from, int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err) |
| bool | calcInverseKinematics (int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err, Eigen::MatrixXd weight) |
| bool | calcInverseKinematics (int from, int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err, Eigen::MatrixXd weight) |
| bool | calcInverseKinematicsForLeftLeg (double *out, double x, double y, double z, double roll, double pitch, double yaw) |
| bool | calcInverseKinematicsForLeg (double *out, double x, double y, double z, double roll, double pitch, double yaw) |
| bool | calcInverseKinematicsForRightLeg (double *out, double x, double y, double z, double roll, double pitch, double yaw) |
| Eigen::MatrixXd | calcJacobian (std::vector< int > idx) |
| Eigen::MatrixXd | calcJacobianCOM (std::vector< int > idx) |
| Eigen::MatrixXd | calcMC (int joint_id) |
| Eigen::MatrixXd | calcPreviewParam (double preview_time, double control_cycle, double lipm_height, Eigen::MatrixXd K, Eigen::MatrixXd P) |
| double | calcTotalMass (int joint_id) |
| Eigen::MatrixXd | calcVWerr (Eigen::MatrixXd tar_position, Eigen::MatrixXd curr_position, Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation) |
| std::vector< int > | findRoute (int to) |
| std::vector< int > | findRoute (int from, int to) |
| Eigen::MatrixXd | getJointAxis (const std::string link_name) |
| double | getJointDirection (const std::string link_name) |
| double | getJointDirection (const int link_id) |
| LinkData * | getLinkData (const std::string link_name) |
| LinkData * | getLinkData (const int link_id) |
| OP3KinematicsDynamics () | |
| OP3KinematicsDynamics (TreeSelect tree) | |
| ~OP3KinematicsDynamics () | |
Public Attributes | |
| double | ankle_length_m_ |
| double | calf_length_m_ |
| double | leg_side_offset_m_ |
| LinkData * | op3_link_data_ [ALL_JOINT_ID+1] |
| double | thigh_length_m_ |
Definition at line 38 of file op3_kinematics_dynamics.h.
| robotis_op::OP3KinematicsDynamics::OP3KinematicsDynamics | ( | ) |
Definition at line 25 of file op3_kinematics_dynamics.cpp.
| robotis_op::OP3KinematicsDynamics::~OP3KinematicsDynamics | ( | ) |
Definition at line 28 of file op3_kinematics_dynamics.cpp.
| robotis_op::OP3KinematicsDynamics::OP3KinematicsDynamics | ( | TreeSelect | tree | ) |
Definition at line 32 of file op3_kinematics_dynamics.cpp.
| Eigen::MatrixXd robotis_op::OP3KinematicsDynamics::calcCOM | ( | Eigen::MatrixXd | mc | ) |
Definition at line 559 of file op3_kinematics_dynamics.cpp.
| void robotis_op::OP3KinematicsDynamics::calcForwardKinematics | ( | int | joint_ID | ) |
Definition at line 570 of file op3_kinematics_dynamics.cpp.
| bool robotis_op::OP3KinematicsDynamics::calcInverseKinematics | ( | int | to, |
| Eigen::MatrixXd | tar_position, | ||
| Eigen::MatrixXd | tar_orientation, | ||
| int | max_iter, | ||
| double | ik_err | ||
| ) |
Definition at line 659 of file op3_kinematics_dynamics.cpp.
| bool robotis_op::OP3KinematicsDynamics::calcInverseKinematics | ( | int | from, |
| int | to, | ||
| Eigen::MatrixXd | tar_position, | ||
| Eigen::MatrixXd | tar_orientation, | ||
| int | max_iter, | ||
| double | ik_err | ||
| ) |
Definition at line 724 of file op3_kinematics_dynamics.cpp.
| bool robotis_op::OP3KinematicsDynamics::calcInverseKinematics | ( | int | to, |
| Eigen::MatrixXd | tar_position, | ||
| Eigen::MatrixXd | tar_orientation, | ||
| int | max_iter, | ||
| double | ik_err, | ||
| Eigen::MatrixXd | weight | ||
| ) |
Definition at line 789 of file op3_kinematics_dynamics.cpp.
| bool robotis_op::OP3KinematicsDynamics::calcInverseKinematics | ( | int | from, |
| int | to, | ||
| Eigen::MatrixXd | tar_position, | ||
| Eigen::MatrixXd | tar_orientation, | ||
| int | max_iter, | ||
| double | ik_err, | ||
| Eigen::MatrixXd | weight | ||
| ) |
Definition at line 874 of file op3_kinematics_dynamics.cpp.
| bool robotis_op::OP3KinematicsDynamics::calcInverseKinematicsForLeftLeg | ( | double * | out, |
| double | x, | ||
| double | y, | ||
| double | z, | ||
| double | roll, | ||
| double | pitch, | ||
| double | yaw | ||
| ) |
Definition at line 1060 of file op3_kinematics_dynamics.cpp.
| bool robotis_op::OP3KinematicsDynamics::calcInverseKinematicsForLeg | ( | double * | out, |
| double | x, | ||
| double | y, | ||
| double | z, | ||
| double | roll, | ||
| double | pitch, | ||
| double | yaw | ||
| ) |
Definition at line 955 of file op3_kinematics_dynamics.cpp.
| bool robotis_op::OP3KinematicsDynamics::calcInverseKinematicsForRightLeg | ( | double * | out, |
| double | x, | ||
| double | y, | ||
| double | z, | ||
| double | roll, | ||
| double | pitch, | ||
| double | yaw | ||
| ) |
Definition at line 1046 of file op3_kinematics_dynamics.cpp.
| Eigen::MatrixXd robotis_op::OP3KinematicsDynamics::calcJacobian | ( | std::vector< int > | idx | ) |
Definition at line 600 of file op3_kinematics_dynamics.cpp.
| Eigen::MatrixXd robotis_op::OP3KinematicsDynamics::calcJacobianCOM | ( | std::vector< int > | idx | ) |
Definition at line 622 of file op3_kinematics_dynamics.cpp.
| Eigen::MatrixXd robotis_op::OP3KinematicsDynamics::calcMC | ( | int | joint_id | ) |
Definition at line 542 of file op3_kinematics_dynamics.cpp.
| Eigen::MatrixXd robotis_op::OP3KinematicsDynamics::calcPreviewParam | ( | double | preview_time, |
| double | control_cycle, | ||
| double | lipm_height, | ||
| Eigen::MatrixXd | K, | ||
| Eigen::MatrixXd | P | ||
| ) |
Definition at line 1139 of file op3_kinematics_dynamics.cpp.
| double robotis_op::OP3KinematicsDynamics::calcTotalMass | ( | int | joint_id | ) |
Definition at line 529 of file op3_kinematics_dynamics.cpp.
| Eigen::MatrixXd robotis_op::OP3KinematicsDynamics::calcVWerr | ( | Eigen::MatrixXd | tar_position, |
| Eigen::MatrixXd | curr_position, | ||
| Eigen::MatrixXd | tar_orientation, | ||
| Eigen::MatrixXd | curr_orientation | ||
| ) |
Definition at line 645 of file op3_kinematics_dynamics.cpp.
| std::vector< int > robotis_op::OP3KinematicsDynamics::findRoute | ( | int | to | ) |
Definition at line 489 of file op3_kinematics_dynamics.cpp.
| std::vector< int > robotis_op::OP3KinematicsDynamics::findRoute | ( | int | from, |
| int | to | ||
| ) |
Definition at line 509 of file op3_kinematics_dynamics.cpp.
| Eigen::MatrixXd robotis_op::OP3KinematicsDynamics::getJointAxis | ( | const std::string | link_name | ) |
Definition at line 1097 of file op3_kinematics_dynamics.cpp.
| double robotis_op::OP3KinematicsDynamics::getJointDirection | ( | const std::string | link_name | ) |
Definition at line 1111 of file op3_kinematics_dynamics.cpp.
| double robotis_op::OP3KinematicsDynamics::getJointDirection | ( | const int | link_id | ) |
Definition at line 1125 of file op3_kinematics_dynamics.cpp.
| LinkData * robotis_op::OP3KinematicsDynamics::getLinkData | ( | const std::string | link_name | ) |
Definition at line 1074 of file op3_kinematics_dynamics.cpp.
| LinkData * robotis_op::OP3KinematicsDynamics::getLinkData | ( | const int | link_id | ) |
Definition at line 1087 of file op3_kinematics_dynamics.cpp.
| double robotis_op::OP3KinematicsDynamics::ankle_length_m_ |
Definition at line 91 of file op3_kinematics_dynamics.h.
| double robotis_op::OP3KinematicsDynamics::calf_length_m_ |
Definition at line 90 of file op3_kinematics_dynamics.h.
| double robotis_op::OP3KinematicsDynamics::leg_side_offset_m_ |
Definition at line 92 of file op3_kinematics_dynamics.h.
| LinkData* robotis_op::OP3KinematicsDynamics::op3_link_data_[ALL_JOINT_ID+1] |
Definition at line 77 of file op3_kinematics_dynamics.h.
| double robotis_op::OP3KinematicsDynamics::thigh_length_m_ |
Definition at line 89 of file op3_kinematics_dynamics.h.