19 #ifndef OP3_ONLINE_WALKING_MODULE_OP3_KDL_ 20 #define OP3_ONLINE_WALKING_MODULE_OP3_KDL_ 32 #include <geometry_msgs/Pose.h> 33 #include <eigen3/Eigen/Eigen> 34 #include <kdl/joint.hpp> 35 #include <kdl/chain.hpp> 36 #include <kdl/chaindynparam.hpp> 37 #include <kdl/jacobian.hpp> 38 #include <kdl/chainjnttojacsolver.hpp> 39 #include <kdl/chainfksolver.hpp> 40 #include <kdl/chainfksolverpos_recursive.hpp> 41 #include <kdl/chainiksolvervel_pinv.hpp> 42 #include <kdl/chainiksolverpos_nr_jl.hpp> 44 #define LEG_JOINT_NUM (6) 45 #define D2R (M_PI/180.0) 54 void initialize(Eigen::MatrixXd pelvis_position, Eigen::MatrixXd pelvis_orientation);
55 void setJointPosition(Eigen::VectorXd rleg_joint_position, Eigen::VectorXd lleg_joint_position);
57 std::vector<double_t> &lleg_position, std::vector<double_t> &lleg_orientation);
59 Eigen::MatrixXd rleg_target_position, Eigen::Quaterniond rleg_target_orientation,
60 std::vector<double_t> &lleg_output,
61 Eigen::MatrixXd lleg_target_position, Eigen::Quaterniond lleg_target_orientation);
void initialize(Eigen::MatrixXd pelvis_position, Eigen::MatrixXd pelvis_orientation)
KDL::ChainFkSolverPos_recursive * lleg_fk_solver_
KDL::ChainJntToJacSolver * lleg_jacobian_solver_
KDL::ChainDynParam * lleg_dyn_param_
Eigen::VectorXd rleg_joint_position_
void solveForwardKinematics(std::vector< double_t > &rleg_position, std::vector< double_t > &rleg_orientation, std::vector< double_t > &lleg_position, std::vector< double_t > &lleg_orientation)
KDL::ChainFkSolverPos_recursive * rleg_ft_fk_solver_
KDL::ChainFkSolverPos_recursive * rleg_fk_solver_
void setJointPosition(Eigen::VectorXd rleg_joint_position, Eigen::VectorXd lleg_joint_position)
KDL::ChainJntToJacSolver * rleg_jacobian_solver_
bool solveInverseKinematics(std::vector< double_t > &rleg_output, Eigen::MatrixXd rleg_target_position, Eigen::Quaterniond rleg_target_orientation, std::vector< double_t > &lleg_output, Eigen::MatrixXd lleg_target_position, Eigen::Quaterniond lleg_target_orientation)
geometry_msgs::Pose rleg_ft_pose_
Eigen::VectorXd lleg_joint_position_
KDL::ChainIkSolverVel_pinv * lleg_ik_vel_solver_
geometry_msgs::Pose lleg_pose_
geometry_msgs::Pose lleg_ft_pose_
geometry_msgs::Pose rleg_pose_
KDL::ChainIkSolverVel_pinv * rleg_ik_vel_solver_
KDL::ChainDynParam * rleg_dyn_param_
KDL::ChainFkSolverPos_recursive * lleg_ft_fk_solver_
KDL::ChainIkSolverPos_NR_JL * rleg_ik_pos_solver_
KDL::ChainIkSolverPos_NR_JL * lleg_ik_pos_solver_