#include <collision_proximity_planner.h>
Definition at line 72 of file collision_proximity_planner.h.
collision_proximity_planner::CollisionProximityPlanner::CollisionProximityPlanner | ( | const std::string & | robot_description_name | ) |
Definition at line 46 of file collision_proximity_planner.cpp.
Definition at line 72 of file collision_proximity_planner.cpp.
bool collision_proximity_planner::CollisionProximityPlanner::calculateCollisionIncrements | ( | Eigen::MatrixXd & | collision_increments, |
double & | distance | ||
) | [private] |
Definition at line 341 of file collision_proximity_planner.cpp.
Definition at line 139 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::fillInGroupArray | ( | const KDL::JntArray & | jnt_array_group, |
const std::vector< int > & | group_joint_to_kdl_joint_index, | ||
KDL::JntArray & | jnt_array | ||
) | [private] |
Definition at line 170 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::fillInGroupState | ( | arm_navigation_msgs::RobotState & | robot_state, |
const arm_navigation_msgs::RobotState & | group_state | ||
) |
Definition at line 146 of file collision_proximity_planner.cpp.
bool collision_proximity_planner::CollisionProximityPlanner::findPathToFreeState | ( | const arm_navigation_msgs::RobotState & | robot_state, |
arm_navigation_msgs::RobotTrajectory & | robot_trajectory | ||
) |
Definition at line 178 of file collision_proximity_planner.cpp.
bool collision_proximity_planner::CollisionProximityPlanner::getFreePath | ( | arm_navigation_msgs::GetMotionPlan::Request & | req, |
arm_navigation_msgs::GetMotionPlan::Response & | res | ||
) | [private] |
Definition at line 108 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::getGroupArray | ( | const KDL::JntArray & | jnt_array, |
const std::vector< int > & | group_joint_to_kdl_joint_index, | ||
KDL::JntArray & | jnt_array_group | ||
) | [private] |
Definition at line 162 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::getJacobian | ( | const int & | link_index, |
std::vector< Eigen::Map< Eigen::Vector3d > > & | joint_pos, | ||
std::vector< Eigen::Map< Eigen::Vector3d > > & | joint_axis, | ||
Eigen::Vector3d & | collision_point_pos, | ||
Eigen::MatrixBase< Derived > & | jacobian, | ||
const std::vector< int > & | group_joint_to_kdl_joint_index | ||
) | const [private] |
Definition at line 193 of file collision_proximity_planner.h.
bool collision_proximity_planner::CollisionProximityPlanner::getStateGradient | ( | const arm_navigation_msgs::RobotState & | group_state, |
double & | distance, | ||
arm_navigation_msgs::RobotState & | gradient | ||
) |
Given a robot state, get the gradient direction to be moved in.
joint_state_group | The group state that needs to be refined |
Definition at line 258 of file collision_proximity_planner.cpp.
bool collision_proximity_planner::CollisionProximityPlanner::initializeForGroup | ( | const std::string & | group_name | ) | [private] |
Definition at line 76 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::isParentJoint | ( | const int & | link_index, |
const int & | joint_index | ||
) | [private] |
bool collision_proximity_planner::CollisionProximityPlanner::isParentJoint | ( | const int & | link_index, |
const int & | joint_index | ||
) | const [inline, private] |
Definition at line 215 of file collision_proximity_planner.h.
void collision_proximity_planner::CollisionProximityPlanner::kdlJointTrajectoryToRobotTrajectory | ( | std::vector< KDL::JntArray > & | jnt_trajectory, |
arm_navigation_msgs::RobotTrajectory & | robot_trajectory | ||
) | [private] |
Definition at line 314 of file collision_proximity_planner.cpp.
bool collision_proximity_planner::CollisionProximityPlanner::mapGroupState | ( | const arm_navigation_msgs::RobotState & | group_state, |
const std::vector< int > & | mapping | ||
) | [private] |
Definition at line 245 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::performForwardKinematics | ( | KDL::JntArray & | jnt_array, |
const bool & | full | ||
) | [private] |
Definition at line 398 of file collision_proximity_planner.cpp.
bool collision_proximity_planner::CollisionProximityPlanner::refineState | ( | const arm_navigation_msgs::RobotState & | group_state, |
arm_navigation_msgs::RobotTrajectory & | robot_trajectory | ||
) |
Definition at line 276 of file collision_proximity_planner.cpp.
bool collision_proximity_planner::CollisionProximityPlanner::setGroupState | ( | const arm_navigation_msgs::RobotState & | group_state | ) |
Set a group state. This must be called before you can make multiple queries to refineState below. This function can be used to define a mapping between the group state specified in the argument and the internal group state for more efficiency.
group_state |
Definition at line 224 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::setPlanningMonitorToCurrentState | ( | ) | [private] |
bool collision_proximity_planner::CollisionProximityPlanner::setRobotState | ( | const arm_navigation_msgs::RobotState & | robot_state | ) |
Set the robot state that you want to check. Note that this locks the collision space and you will have to call clear() to unlock the space.
robot_state | The full robot state. |
void collision_proximity_planner::CollisionProximityPlanner::updateCollisionProximitySpace | ( | const arm_navigation_msgs::RobotState & | group_state | ) | [private] |
Definition at line 335 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::updateGroupRobotState | ( | const KDL::JntArray & | jnt_array | ) | [private] |
Definition at line 330 of file collision_proximity_planner.cpp.
void collision_proximity_planner::CollisionProximityPlanner::updateJointState | ( | KDL::JntArray & | jnt_array, |
Eigen::MatrixXd & | collision_increments | ||
) | [private] |
Definition at line 410 of file collision_proximity_planner.cpp.
std::vector<std::vector<int> > collision_proximity_planner::CollisionProximityPlanner::active_joints_ [private] |
Definition at line 171 of file collision_proximity_planner.h.
chomp::ChompRobotModel collision_proximity_planner::CollisionProximityPlanner::chomp_robot_model_ [private] |
Definition at line 144 of file collision_proximity_planner.h.
Eigen::MatrixXd collision_proximity_planner::CollisionProximityPlanner::collision_increments_ [private] |
Definition at line 156 of file collision_proximity_planner.h.
std::vector<KDL::Vector> collision_proximity_planner::CollisionProximityPlanner::collision_point_pos_ [private] |
Definition at line 161 of file collision_proximity_planner.h.
std::vector<Eigen::Map<Eigen::Vector3d> > collision_proximity_planner::CollisionProximityPlanner::collision_point_pos_eigen_ [private] |
Definition at line 165 of file collision_proximity_planner.h.
std::vector<Eigen::Vector3d> collision_proximity_planner::CollisionProximityPlanner::collision_point_potential_gradient_ [private] |
Definition at line 167 of file collision_proximity_planner.h.
std::vector<double> collision_proximity_planner::CollisionProximityPlanner::collision_point_vel_mag_ [private] |
Definition at line 166 of file collision_proximity_planner.h.
collision_proximity::CollisionProximitySpace* collision_proximity_planner::CollisionProximityPlanner::cps_ [private] |
Definition at line 141 of file collision_proximity_planner.h.
std::vector<std::string> collision_proximity_planner::CollisionProximityPlanner::current_attached_body_names_ [private] |
Definition at line 150 of file collision_proximity_planner.h.
std::vector<std::string> collision_proximity_planner::CollisionProximityPlanner::current_link_names_ [private] |
Definition at line 149 of file collision_proximity_planner.h.
ros::Publisher collision_proximity_planner::CollisionProximityPlanner::display_trajectory_publisher_ [private] |
Definition at line 117 of file collision_proximity_planner.h.
std::vector<int> collision_proximity_planner::CollisionProximityPlanner::group_joint_to_kdl_joint_index_ [private] |
Definition at line 169 of file collision_proximity_planner.h.
std::string collision_proximity_planner::CollisionProximityPlanner::group_name_cps_ [private] |
Definition at line 142 of file collision_proximity_planner.h.
std::vector<int> collision_proximity_planner::CollisionProximityPlanner::group_state_joint_array_group_mapping_ [private] |
Definition at line 189 of file collision_proximity_planner.h.
Eigen::MatrixXd collision_proximity_planner::CollisionProximityPlanner::jacobian_ [private] |
Definition at line 153 of file collision_proximity_planner.h.
Eigen::MatrixXd collision_proximity_planner::CollisionProximityPlanner::jacobian_jacobian_tranpose_ [private] |
Definition at line 155 of file collision_proximity_planner.h.
Eigen::MatrixXd collision_proximity_planner::CollisionProximityPlanner::jacobian_pseudo_inverse_ [private] |
Definition at line 154 of file collision_proximity_planner.h.
KDL::JntArray collision_proximity_planner::CollisionProximityPlanner::jnt_array_ [private] |
Definition at line 188 of file collision_proximity_planner.h.
KDL::JntArray collision_proximity_planner::CollisionProximityPlanner::jnt_array_group_ [private] |
Definition at line 188 of file collision_proximity_planner.h.
std::vector<int> collision_proximity_planner::CollisionProximityPlanner::joint_array_group_group_state_mapping_ [private] |
Definition at line 189 of file collision_proximity_planner.h.
std::vector<KDL::Vector> collision_proximity_planner::CollisionProximityPlanner::joint_axis_ [private] |
Definition at line 158 of file collision_proximity_planner.h.
std::vector<Eigen::Map<Eigen::Vector3d> > collision_proximity_planner::CollisionProximityPlanner::joint_axis_eigen_ [private] |
Definition at line 163 of file collision_proximity_planner.h.
std::vector<KDL::Vector> collision_proximity_planner::CollisionProximityPlanner::joint_pos_ [private] |
Definition at line 159 of file collision_proximity_planner.h.
std::vector<Eigen::Map<Eigen::Vector3d> > collision_proximity_planner::CollisionProximityPlanner::joint_pos_eigen_ [private] |
Definition at line 164 of file collision_proximity_planner.h.
Definition at line 146 of file collision_proximity_planner.h.
Definition at line 147 of file collision_proximity_planner.h.
Definition at line 143 of file collision_proximity_planner.h.
const chomp::ChompRobotModel::ChompPlanningGroup* collision_proximity_planner::CollisionProximityPlanner::planning_group_ [private] |
Definition at line 145 of file collision_proximity_planner.h.
ros::ServiceServer collision_proximity_planner::CollisionProximityPlanner::planning_service_ [private] |
Definition at line 186 of file collision_proximity_planner.h.
Definition at line 140 of file collision_proximity_planner.h.
std::string collision_proximity_planner::CollisionProximityPlanner::reference_frame_ [private] |
Definition at line 142 of file collision_proximity_planner.h.
arm_navigation_msgs::RobotState collision_proximity_planner::CollisionProximityPlanner::robot_state_group_ [private] |
Definition at line 138 of file collision_proximity_planner.h.
Definition at line 140 of file collision_proximity_planner.h.
std::vector<KDL::Frame> collision_proximity_planner::CollisionProximityPlanner::segment_frames_ [private] |
Definition at line 160 of file collision_proximity_planner.h.
Definition at line 152 of file collision_proximity_planner.h.
ros::Publisher collision_proximity_planner::CollisionProximityPlanner::vis_marker_array_publisher_ [private] |
Definition at line 173 of file collision_proximity_planner.h.
ros::Publisher collision_proximity_planner::CollisionProximityPlanner::vis_marker_publisher_ [private] |
Definition at line 173 of file collision_proximity_planner.h.