PIK ROS
|
Prioritized inverse kinematics solver. More...
#include <PikRos.hpp>
Public Member Functions | |
Pik (ros::NodeHandle &nh_, const std::string &robot_name, const std::string &joint_model_group_name, const Settings &settings=Settings()) | |
Construct a new Pik object. More... | |
Eigen::VectorXd | solve (const std::vector< IkTask > &ik_tasks, const Eigen::VectorXd &q_initial_guess) |
Solves a prioritized inverse kinematics problem. More... | |
void | goToJointPosition (const Eigen::VectorXd &q) const |
Moves the robot to desired joint positions. More... | |
void | printErrors (const Eigen::VectorXd &q, const std::vector< IkTask > &ik_tasks) const |
Prints task errors for a given joint position vector. More... | |
Prioritized inverse kinematics solver.
Solves the positional inverse kinematics problem given a vector of Ik tasks with descending priorities
Pik::Pik | ( | ros::NodeHandle & | nh_, |
const std::string & | robot_name, | ||
const std::string & | joint_model_group_name, | ||
const Settings & | settings = Settings() |
||
) |
Construct a new Pik object.
nh_ | ROS node handle |
robot_name | Namespace for robot description |
joint_model_group_name | Joint model group name in MoveIt |
settings | PikRos::Settings object |
void Pik::goToJointPosition | ( | const Eigen::VectorXd & | q | ) | const |
Moves the robot to desired joint positions.
q | Eigen::VectorXd Joint position vector |
void Pik::printErrors | ( | const Eigen::VectorXd & | q, |
const std::vector< IkTask > & | ik_tasks | ||
) | const |
Prints task errors for a given joint position vector.
q | Eigen::VectorXd Joint position vector |
ik_tasks | std::vector<IkTask> Inverse kinematics tasks; |
Eigen::VectorXd Pik::solve | ( | const std::vector< IkTask > & | ik_tasks, |
const Eigen::VectorXd & | q_initial_guess | ||
) |
Solves a prioritized inverse kinematics problem.
ik_tasks | Ik tasks std::vector<IkTask> from higher priority to lower |
q_initial_guess | Initial guess for a joint position vector q |