#include <chomp_collision_point.h>
Public Member Functions | |
| ChompCollisionPoint (const ChompCollisionPoint &point, const std::vector< int > &parent_joints) | |
| ChompCollisionPoint (const std::vector< int > &parent_joints, double radius, double clearance, int segment_number, const KDL::Vector &position) | |
| double | getClearance () const |
| double | getInvClearance () const |
| template<typename Derived > | |
| void | getJacobian (std::vector< Eigen::Map< Eigen::Vector3d > > &joint_pos, std::vector< Eigen::Map< Eigen::Vector3d > > &joint_axis, Eigen::Map< Eigen::Vector3d > &collision_point_pos, Eigen::MatrixBase< Derived > &jacobian, const std::vector< int > &group_joint_to_kdl_joint_index) const |
| const std::vector< int > & | getParentJoints () const |
| const KDL::Vector & | getPosition () const |
| double | getRadius () const |
| int | getSegmentNumber () const |
| void | getTransformedPosition (std::vector< KDL::Frame > &segment_frames, KDL::Vector &position) const |
| double | getVolume () const |
| bool | isParentJoint (int joint) const |
| virtual | ~ChompCollisionPoint () |
Private Attributes | |
| double | clearance_ |
| double | inv_clearance_ |
| std::vector< int > | parent_joints_ |
| KDL::Vector | position_ |
| double | radius_ |
| int | segment_number_ |
| double | volume_ |
Definition at line 43 of file chomp_collision_point.h.
| chomp::ChompCollisionPoint::ChompCollisionPoint | ( | const std::vector< int > & | parent_joints, | |
| double | radius, | |||
| double | clearance, | |||
| int | segment_number, | |||
| const KDL::Vector & | position | |||
| ) |
Definition at line 44 of file chomp_collision_point.cpp.
| chomp::ChompCollisionPoint::ChompCollisionPoint | ( | const ChompCollisionPoint & | point, | |
| const std::vector< int > & | parent_joints | |||
| ) |
Definition at line 57 of file chomp_collision_point.cpp.
| chomp::ChompCollisionPoint::~ChompCollisionPoint | ( | ) | [virtual] |
Definition at line 68 of file chomp_collision_point.cpp.
| double chomp::ChompCollisionPoint::getClearance | ( | ) | const [inline] |
Definition at line 94 of file chomp_collision_point.h.
| double chomp::ChompCollisionPoint::getInvClearance | ( | ) | const [inline] |
Definition at line 99 of file chomp_collision_point.h.
| void chomp::ChompCollisionPoint::getJacobian | ( | std::vector< Eigen::Map< Eigen::Vector3d > > & | joint_pos, | |
| std::vector< Eigen::Map< Eigen::Vector3d > > & | joint_axis, | |||
| Eigen::Map< Eigen::Vector3d > & | collision_point_pos, | |||
| Eigen::MatrixBase< Derived > & | jacobian, | |||
| const std::vector< int > & | group_joint_to_kdl_joint_index | |||
| ) | const [inline] |
Definition at line 115 of file chomp_collision_point.h.
| const std::vector<int>& chomp::ChompCollisionPoint::getParentJoints | ( | ) | const [inline] |
Definition at line 56 of file chomp_collision_point.h.
| const KDL::Vector & chomp::ChompCollisionPoint::getPosition | ( | ) | const [inline] |
Definition at line 109 of file chomp_collision_point.h.
| double chomp::ChompCollisionPoint::getRadius | ( | ) | const [inline] |
Definition at line 84 of file chomp_collision_point.h.
| int chomp::ChompCollisionPoint::getSegmentNumber | ( | ) | const [inline] |
Definition at line 104 of file chomp_collision_point.h.
| void chomp::ChompCollisionPoint::getTransformedPosition | ( | std::vector< KDL::Frame > & | segment_frames, | |
| KDL::Vector & | position | |||
| ) | const [inline] |
Definition at line 132 of file chomp_collision_point.h.
| double chomp::ChompCollisionPoint::getVolume | ( | ) | const [inline] |
Definition at line 89 of file chomp_collision_point.h.
| bool chomp::ChompCollisionPoint::isParentJoint | ( | int | joint | ) | const [inline] |
Definition at line 79 of file chomp_collision_point.h.
double chomp::ChompCollisionPoint::clearance_ [private] |
Extra clearance required while optimizing
Definition at line 70 of file chomp_collision_point.h.
double chomp::ChompCollisionPoint::inv_clearance_ [private] |
1/clearance_ pre-computed
Definition at line 71 of file chomp_collision_point.h.
std::vector<int> chomp::ChompCollisionPoint::parent_joints_ [private] |
Which joints can influence the motion of this point
Definition at line 67 of file chomp_collision_point.h.
KDL::Vector chomp::ChompCollisionPoint::position_ [private] |
Vector of this point in the frame of the above segment
Definition at line 73 of file chomp_collision_point.h.
double chomp::ChompCollisionPoint::radius_ [private] |
Radius of the sphere
Definition at line 68 of file chomp_collision_point.h.
int chomp::ChompCollisionPoint::segment_number_ [private] |
Which segment does this point belong to
Definition at line 72 of file chomp_collision_point.h.
double chomp::ChompCollisionPoint::volume_ [private] |
Volume of the sphere
Definition at line 69 of file chomp_collision_point.h.