#include <cob_articulation.h>
Public Member Functions | |
void | calculateProfile (std::vector< double > *, double, double, double, std::string) |
void | calculateProfileForAngularMovements (std::vector< double > *pathMatrix, double, double, double, double, double, double, double, double, double, std::string, bool) |
void | circular_interpolation (std::vector< geometry_msgs::Pose > *, double, double, double, double, double, double, double, double, double, double, double, std::string) |
bool | epsilon_area (double, double, double, double, double, double, double) |
void | generatePath (std::vector< double > *, double, double, double, double, int, std::string) |
void | generatePathWithTe (std::vector< double > *pathArray, double T_IPO, double te, double AcclMax, double Se_max, int steps_max, double start_angle, std::string profile) |
geometry_msgs::Pose | getEndeffectorPose () |
void | hold_position (geometry_msgs::Pose) |
bool | initialize () |
void | linear_interpolation (std::vector< geometry_msgs::Pose > *poseVector, geometry_msgs::Pose, geometry_msgs::Pose, double, double, std::string, bool) |
void | load () |
void | move_ptp (geometry_msgs::Pose targetPose, double epsilon) |
void | pose_path_broadcaster (std::vector< geometry_msgs::Pose > *) |
void | PoseToRPY (geometry_msgs::Pose pose, double &roll, double &pitch, double &yaw) |
void | showDot (double, double, double, int, double, double, double, std::string) |
void | showLevel (tf::Transform, int, double, double, double, std::string) |
void | showMarker (tf::StampedTransform, int, double, double, double, std::string) |
void | start_tracking () |
void | stop_tracking () |
void | timerCallback (const ros::TimerEvent &) |
Private Attributes | |
ros::Publisher | accl_pub_ |
tf::TransformBroadcaster | br_ |
const char * | charPath_ |
tf::StampedTransform | currentEndeffectorStampedTransform_ |
std::string | endeffectorFrame_ |
std::string | fileName_ |
bool | hold_ |
ros::Publisher | jerk_pub_ |
tf::TransformListener | listener_ |
int | marker1_ |
int | marker2_ |
ros::NodeHandle | nh_ |
ros::Publisher | path_pub_ |
tf::Quaternion | q_ |
bool | reached_pos_ |
std::string | referenceFrame_ |
ros::Publisher | speed_pub_ |
ros::ServiceClient | startTracking_ |
ros::ServiceClient | stopTracking_ |
std::string | stringPath_ |
std::string | targetFrame_ |
tf::Transform | transform_ |
double | update_rate_ |
ros::Publisher | vis_pub_ |
Definition at line 41 of file cob_articulation.h.
void CobArticulation::calculateProfile | ( | std::vector< double > * | pathArray, |
double | Se, | ||
double | VelMax, | ||
double | AcclMax, | ||
std::string | profile | ||
) |
Definition at line 533 of file cob_articulation.cpp.
void CobArticulation::calculateProfileForAngularMovements | ( | std::vector< double > * | pathMatrix, |
double | Se, | ||
double | Se_roll, | ||
double | Se_pitch, | ||
double | Se_yaw, | ||
double | start_angle_roll, | ||
double | start_angle_pitch, | ||
double | start_angle_yaw, | ||
double | VelMax, | ||
double | AcclMax, | ||
std::string | profile, | ||
bool | justRotate | ||
) |
Definition at line 614 of file cob_articulation.cpp.
void CobArticulation::circular_interpolation | ( | std::vector< geometry_msgs::Pose > * | poseVector, |
double | M_x, | ||
double | M_y, | ||
double | M_z, | ||
double | M_roll, | ||
double | M_pitch, | ||
double | M_yaw, | ||
double | startAngle, | ||
double | endAngle, | ||
double | r, | ||
double | VelMax, | ||
double | AcclMax, | ||
std::string | profile | ||
) |
Definition at line 454 of file cob_articulation.cpp.
bool CobArticulation::epsilon_area | ( | double | x, |
double | y, | ||
double | z, | ||
double | roll, | ||
double | pitch, | ||
double | yaw, | ||
double | epsilon | ||
) |
Definition at line 766 of file cob_articulation.cpp.
void CobArticulation::generatePath | ( | std::vector< double > * | pathArray, |
double | T_IPO, | ||
double | VelMax, | ||
double | AcclMax, | ||
double | Se_max, | ||
int | steps_max, | ||
std::string | profile | ||
) |
Definition at line 918 of file cob_articulation.cpp.
void CobArticulation::generatePathWithTe | ( | std::vector< double > * | pathArray, |
double | T_IPO, | ||
double | te, | ||
double | AcclMax, | ||
double | Se_max, | ||
int | steps_max, | ||
double | start_angle, | ||
std::string | profile | ||
) |
Definition at line 984 of file cob_articulation.cpp.
Definition at line 732 of file cob_articulation.cpp.
void CobArticulation::hold_position | ( | geometry_msgs::Pose | holdPose | ) |
Definition at line 361 of file cob_articulation.cpp.
bool CobArticulation::initialize | ( | ) |
get params
Definition at line 43 of file cob_articulation.cpp.
void CobArticulation::linear_interpolation | ( | std::vector< geometry_msgs::Pose > * | poseVector, |
geometry_msgs::Pose | start, | ||
geometry_msgs::Pose | end, | ||
double | VelMax, | ||
double | AcclMax, | ||
std::string | profile, | ||
bool | justRotate | ||
) |
Definition at line 386 of file cob_articulation.cpp.
void CobArticulation::load | ( | ) |
Definition at line 105 of file cob_articulation.cpp.
void CobArticulation::move_ptp | ( | geometry_msgs::Pose | targetPose, |
double | epsilon | ||
) |
Definition at line 271 of file cob_articulation.cpp.
void CobArticulation::pose_path_broadcaster | ( | std::vector< geometry_msgs::Pose > * | poseVector | ) |
Definition at line 326 of file cob_articulation.cpp.
void CobArticulation::PoseToRPY | ( | geometry_msgs::Pose | pose, |
double & | roll, | ||
double & | pitch, | ||
double & | yaw | ||
) |
Definition at line 1059 of file cob_articulation.cpp.
void CobArticulation::showDot | ( | double | x, |
double | y, | ||
double | z, | ||
int | marker_id, | ||
double | red, | ||
double | green, | ||
double | blue, | ||
std::string | ns | ||
) |
Definition at line 825 of file cob_articulation.cpp.
void CobArticulation::showLevel | ( | tf::Transform | pos, |
int | marker_id, | ||
double | red, | ||
double | green, | ||
double | blue, | ||
std::string | ns | ||
) |
Definition at line 855 of file cob_articulation.cpp.
void CobArticulation::showMarker | ( | tf::StampedTransform | tf, |
int | marker_id, | ||
double | red, | ||
double | green, | ||
double | blue, | ||
std::string | ns | ||
) |
Definition at line 794 of file cob_articulation.cpp.
void CobArticulation::start_tracking | ( | ) |
Definition at line 891 of file cob_articulation.cpp.
void CobArticulation::stop_tracking | ( | ) |
Definition at line 906 of file cob_articulation.cpp.
void CobArticulation::timerCallback | ( | const ros::TimerEvent & | event | ) |
Definition at line 886 of file cob_articulation.cpp.
ros::Publisher CobArticulation::accl_pub_ [private] |
Definition at line 77 of file cob_articulation.h.
tf::TransformBroadcaster CobArticulation::br_ [private] |
Definition at line 83 of file cob_articulation.h.
const char* CobArticulation::charPath_ [private] |
Definition at line 96 of file cob_articulation.h.
Definition at line 87 of file cob_articulation.h.
std::string CobArticulation::endeffectorFrame_ [private] |
Definition at line 95 of file cob_articulation.h.
std::string CobArticulation::fileName_ [private] |
Definition at line 95 of file cob_articulation.h.
bool CobArticulation::hold_ [private] |
Definition at line 90 of file cob_articulation.h.
ros::Publisher CobArticulation::jerk_pub_ [private] |
Definition at line 78 of file cob_articulation.h.
Definition at line 86 of file cob_articulation.h.
int CobArticulation::marker1_ [private] |
Definition at line 99 of file cob_articulation.h.
int CobArticulation::marker2_ [private] |
Definition at line 99 of file cob_articulation.h.
ros::NodeHandle CobArticulation::nh_ [private] |
Definition at line 71 of file cob_articulation.h.
ros::Publisher CobArticulation::path_pub_ [private] |
Definition at line 75 of file cob_articulation.h.
tf::Quaternion CobArticulation::q_ [private] |
Definition at line 85 of file cob_articulation.h.
bool CobArticulation::reached_pos_ [private] |
Definition at line 90 of file cob_articulation.h.
std::string CobArticulation::referenceFrame_ [private] |
Definition at line 95 of file cob_articulation.h.
ros::Publisher CobArticulation::speed_pub_ [private] |
Definition at line 76 of file cob_articulation.h.
Definition at line 79 of file cob_articulation.h.
Definition at line 80 of file cob_articulation.h.
std::string CobArticulation::stringPath_ [private] |
Definition at line 95 of file cob_articulation.h.
std::string CobArticulation::targetFrame_ [private] |
Definition at line 95 of file cob_articulation.h.
tf::Transform CobArticulation::transform_ [private] |
Definition at line 84 of file cob_articulation.h.
double CobArticulation::update_rate_ [private] |
Definition at line 94 of file cob_articulation.h.
ros::Publisher CobArticulation::vis_pub_ [private] |
Definition at line 74 of file cob_articulation.h.