From af90c64ee51a57f35e50d006b1b2fee793da3ab8 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Thu, 1 Oct 2015 14:14:48 +0200 Subject: [PATCH 01/35] restructured, introducing base class for profile generator --- cob_cartesian_controller/CMakeLists.txt | 2 +- .../action/CartesianController.action | 2 +- .../cartesian_controller.h | 1 + .../cartesian_controller_data_types.h | 141 +++++- .../trajectory_interpolator.h | 22 +- .../trajactory_profile_generator.h | 93 ++++ .../trajactory_profile_generator_base.h | 60 +++ .../trajectory_profile_generator_circ.h | 59 --- .../trajectory_profile_generator_lin.h | 69 --- cob_cartesian_controller/msg/MoveCirc.msg | 2 - cob_cartesian_controller/msg/MoveLin.msg | 2 - .../src/cartesian_controller.cpp | 52 +- .../trajectory_interpolator.cpp | 56 +-- .../trajectory_profile_generator.cpp | 397 +++++++++++++++ .../trajectory_profile_generator_circ.cpp | 162 ------- .../trajectory_profile_generator_lin.cpp | 456 ------------------ 16 files changed, 756 insertions(+), 820 deletions(-) create mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h create mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h delete mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_circ.h delete mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_lin.h create mode 100644 cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp delete mode 100644 cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_circ.cpp delete mode 100644 cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_lin.cpp diff --git a/cob_cartesian_controller/CMakeLists.txt b/cob_cartesian_controller/CMakeLists.txt index 497f30d5..ef437283 100644 --- a/cob_cartesian_controller/CMakeLists.txt +++ b/cob_cartesian_controller/CMakeLists.txt @@ -38,7 +38,7 @@ add_library(cartesian_controller_utils src/cartesian_controller_utils.cpp) add_dependencies(cartesian_controller_utils ${catkin_EXPORTED_TARGETS}) target_link_libraries(cartesian_controller_utils ${catkin_LIBRARIES}) -add_library(profile_generator src/trajectory_profile_generator/trajectory_profile_generator_lin.cpp src/trajectory_profile_generator/trajectory_profile_generator_circ.cpp) +add_library(profile_generator src/trajectory_profile_generator/trajectory_profile_generator.cpp) add_dependencies(profile_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(profile_generator ${catkin_LIBRARIES}) diff --git a/cob_cartesian_controller/action/CartesianController.action b/cob_cartesian_controller/action/CartesianController.action index 9a0f786a..b389f4df 100644 --- a/cob_cartesian_controller/action/CartesianController.action +++ b/cob_cartesian_controller/action/CartesianController.action @@ -5,7 +5,7 @@ uint8 move_type cob_cartesian_controller/MoveLin move_lin cob_cartesian_controller/MoveCirc move_circ - +cob_cartesian_controller/Profile profile --- # result definition bool success diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h index 46a69c8a..2ccd9dd7 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h @@ -96,6 +96,7 @@ class CartesianController CartesianControllerUtils utils_; boost::shared_ptr< TrajectoryInterpolator > trajectory_interpolator_; + }; #endif diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 02804ab2..5f77b418 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -37,16 +37,22 @@ namespace cob_cartesian_controller { struct ProfileStruct { + double t_ipo; unsigned int profile_type; double vel, accl; + double Se_max; + }; + + struct ProfileTimings + { + double tb, te, tv; + unsigned int steps_tb, steps_te, steps_tv, max_steps; }; struct MoveLinStruct { geometry_msgs::Pose start, end; bool rotate_only; - - ProfileStruct profile; }; struct MoveCircStruct @@ -55,8 +61,6 @@ namespace cob_cartesian_controller double start_angle, end_angle; double radius; bool rotate_only; - - ProfileStruct profile; }; struct CartesianActionStruct @@ -64,9 +68,136 @@ namespace cob_cartesian_controller unsigned int move_type; MoveLinStruct move_lin; MoveCircStruct move_circ; + ProfileStruct profile; }; -}//namespace + class PathArray{ + public: + PathArray(double idx, double Se, double start_value, std::vector array): + Se_(Se), + array_(array), + start_value_(start_value) + { + idx_= idx; + calcTe_=false; + } + + ~PathArray() + { + array_.clear(); + } + + unsigned int getIdx(); + bool getCalcTe(); + double getSe(); + std::vector getArray(); + void setCalcTe(bool calcTe); + void setIdx(unsigned int idx); + void setArray(std::vector array); + double getStartValue(); + + private: + unsigned int idx_; + bool calcTe_; + double Se_; + std::vector array_; + double start_value_; + + }; + + inline double PathArray::getStartValue() + { + return start_value_; + } + + inline void PathArray::setArray(std::vector array) + { + array_ = array; + } + + inline void PathArray::setCalcTe(bool calcTe) + { + calcTe_ = calcTe; + } + + inline void PathArray::setIdx(unsigned int idx) + { + idx_=idx; + } + + inline unsigned int PathArray::getIdx() + { + return idx_; + } + + inline bool PathArray::getCalcTe() + { + return calcTe_; + } + + inline double PathArray::getSe() + { + return Se_; + } + + inline std::vector PathArray::getArray() + { + return array_; + } + + + class PathMatrix{ + public: + PathMatrix(PathArray &pa1, + PathArray &pa2, + PathArray &pa3, + PathArray &pa4) + { + pm_.push_back(pa1); + pm_.push_back(pa2); + pm_.push_back(pa3); + pm_.push_back(pa4); + } + ~PathMatrix() + { + pm_.clear(); + } + + std::vector getSortedMatrix(); + + private: + void sortMatrixRows(); + std::vector pm_; + + }; + + inline std::vector PathMatrix::getSortedMatrix() + { + sortMatrixRows(); + return pm_; + } + + inline void PathMatrix::sortMatrixRows() + { + std::vector tmp; + PathArray temp(0, 0, 0.0, tmp); + for(int j = 0; j<4; j++) + { + for(int i = 0; i<3; i++) + { + if(pm_[i].getSe() < pm_[i+1].getSe()) + { + temp = pm_[i]; + pm_[i] = pm_[i+1]; + pm_[i+1] = temp; + } + } + } + pm_[0].setCalcTe(true); // Reference Time for profile interpolation (Te) + } + + +}//namespace #endif /* COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_ */ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h index d4f7bd97..729198c5 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h @@ -34,32 +34,32 @@ #include #include -#include -#include +#include + class TrajectoryInterpolator { public: TrajectoryInterpolator(std::string root_frame, double update_rate) - : root_frame_(root_frame), - trajectory_profile_generator_lin_(TrajectoryProfileGeneratorLin(update_rate)), - trajectory_profile_generator_circ_(TrajectoryProfileGeneratorCirc(update_rate)) + : root_frame_(root_frame) + {} - ~TrajectoryInterpolator(){} + ~TrajectoryInterpolator(){ + trajectory_profile_generator_.reset(); + } bool linearInterpolation(geometry_msgs::PoseArray& pose_array, - cob_cartesian_controller::MoveLinStruct& move_lin); + const cob_cartesian_controller::CartesianActionStruct& as); bool circularInterpolation(geometry_msgs::PoseArray& pose_array, - cob_cartesian_controller::MoveCircStruct& move_circ); + const cob_cartesian_controller::CartesianActionStruct& as); private: - TrajectoryProfileGeneratorLin trajectory_profile_generator_lin_; - TrajectoryProfileGeneratorCirc trajectory_profile_generator_circ_; - std::string root_frame_; + boost::shared_ptr< TrajectoryProfileBase > trajectory_profile_generator_; + }; #endif /* COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H_ */ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h new file mode 100644 index 00000000..f22cb874 --- /dev/null +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h @@ -0,0 +1,93 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_cartesian_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: September, 2015 + * + * \brief + * + ****************************************************************/ +#ifndef TRAJECTORY_PROFILE_BUILDER_H_ +#define TRAJECTORY_PROFILE_BUILDER_H_ + +#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h" +#include "cob_cartesian_controller/cartesian_controller_data_types.h" + + +/* BEGIN TrajectoryProfileBuilder *****************************************************************************************/ +class TrajectoryProfileBuilder +{ + public: + TrajectoryProfileBuilder() {} + ~TrajectoryProfileBuilder() {} + + static TrajectoryProfileBase* createProfile(const cob_cartesian_controller::CartesianActionStruct& params); +}; +/* END TrajectoryGeneratorBuilder *******************************************************************************************/ + + +/* BEGIN TrajectoryProfileRamp ****************************************************************************************/ +/// Class providing a HardwareInterface publishing velocities. +class TrajectoryProfileRamp: public TrajectoryProfileBase +{ + public: + TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) + : TrajectoryProfileBase(params) + { } + + ~TrajectoryProfileRamp() {} + + virtual void getProfileTimings(double Se_max, double accl, double vel); + virtual void generatePath(cob_cartesian_controller::PathArray &pa); + virtual bool calculateProfile(std::vector* path_matrix, + double Se, double Se_roll, double Se_pitch, double Se_yaw, + geometry_msgs::Pose start); + + + + protected: + cob_cartesian_controller::ProfileTimings pt_; +}; +/* END TrajectoryProfileRamp **********************************************************************************************/ + + +/* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ +/// Class providing a HardwareInterface publishing positions. +class TrajectoryProfileSinoid : public TrajectoryProfileBase +{ + public: + TrajectoryProfileSinoid(const cob_cartesian_controller::CartesianActionStruct& params) + : TrajectoryProfileBase(params) + { } + + ~TrajectoryProfileSinoid() {} + + virtual void getProfileTimings(double Se_max, double accl, double vel); + virtual void generatePath(cob_cartesian_controller::PathArray &pa); + virtual bool calculateProfile(std::vector* path_matrix, + double Se, double Se_roll, double Se_pitch, double Se_yaw, + geometry_msgs::Pose start); + + protected: + cob_cartesian_controller::ProfileTimings pt_; +}; +/* END TrajectoryProfileSinoid **********************************************************************************************/ + +#endif diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h new file mode 100644 index 00000000..b1a2a188 --- /dev/null +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h @@ -0,0 +1,60 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_twist_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: June, 2015 + * + * \brief + * This header contains the interface description of all available + * hardware interface types (position/velocity). + * + ****************************************************************/ +#ifndef HARDWARE_INTERFACE_TYPE_BASE_H_ +#define HARDWARE_INTERFACE_TYPE_BASE_H_ + +#include "ros/ros.h" +#include "cob_cartesian_controller/cartesian_controller_data_types.h" +#include + +/// Base class for hardware interfaces types. +class TrajectoryProfileBase +{ + public: + TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct& params): + params_(params) + {vel_max_=0;} + + virtual ~TrajectoryProfileBase() {} + + + virtual bool calculateProfile(std::vector path_matrix[4], + double Se, double Se_roll, double Se_pitch, double Se_yaw, + geometry_msgs::Pose start) = 0; + virtual void getProfileTimings(double Se_max, double accl, double vel) = 0; + + private: + virtual void generatePath(cob_cartesian_controller::PathArray &pa) = 0; + protected: + const cob_cartesian_controller::CartesianActionStruct ¶ms_; + double vel_max_; +}; + + +#endif /* HARDWARE_INTERFACE_TYPE_BASE_H_ */ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_circ.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_circ.h deleted file mode 100644 index fdaeb42e..00000000 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_circ.h +++ /dev/null @@ -1,59 +0,0 @@ -/*! - ***************************************************************** - * \file - * - * \note - * Copyright (c) 2015 \n - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) \n\n - * - ***************************************************************** - * - * \note - * Project name: care-o-bot - * \note - * ROS stack name: cob_control - * \note - * ROS package name: cob_cartesian_controller - * - * \author - * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com - * - * \date Date of creation: July, 2015 - * - * \brief - * ... - * - ****************************************************************/ - -#ifndef COB_CARTESIAN_CONTROLLER_PROFILE_GENERATOR_CIRC_H_ -#define COB_CARTESIAN_CONTROLLER_PROFILE_GENERATOR_CIRC_H_ - -#include -#include -#include - - -class TrajectoryProfileGeneratorCirc -{ -public: - TrajectoryProfileGeneratorCirc(double update_rate) - { - update_rate_ = update_rate; - t_ipo_ = 1.0/update_rate; - } - - ~TrajectoryProfileGeneratorCirc() - {} - - bool calculateProfile(std::vector& path_array, const double Se, cob_cartesian_controller::ProfileStruct& profile); - -private: - bool calculateRampProfile(std::vector& path_array, const double Se, cob_cartesian_controller::ProfileStruct& profile); - bool calculateSinoidProfile(std::vector& path_array, const double Se, cob_cartesian_controller::ProfileStruct& profile); - - double update_rate_; - double t_ipo_; -}; - -#endif /* COB_CARTESIAN_CONTROLLER_PROFILE_GENERATOR_CIRC_H_ */ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_lin.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_lin.h deleted file mode 100644 index 392d4118..00000000 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_lin.h +++ /dev/null @@ -1,69 +0,0 @@ -/*! - ***************************************************************** - * \file - * - * \note - * Copyright (c) 2015 \n - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) \n\n - * - ***************************************************************** - * - * \note - * Project name: care-o-bot - * \note - * ROS stack name: cob_control - * \note - * ROS package name: cob_cartesian_controller - * - * \author - * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com - * - * \date Date of creation: July, 2015 - * - * \brief - * ... - * - ****************************************************************/ - -#ifndef COB_CARTESIAN_CONTROLLER_PROFILE_GENERATOR_LIN_H_ -#define COB_CARTESIAN_CONTROLLER_PROFILE_GENERATOR_LIN_H_ - -#include -#include -#include - -#define LIN_INDEX 0u -#define ROLL_INDEX 1u -#define PITCH_INDEX 2u -#define YAW_INDEX 3u - -class TrajectoryProfileGeneratorLin -{ -public: - TrajectoryProfileGeneratorLin(double update_rate) - { - update_rate_ = update_rate; - t_ipo_ = 1.0/update_rate; - } - - ~TrajectoryProfileGeneratorLin() - {} - - bool calculateProfile(std::vector* path_matrix, const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, cob_cartesian_controller::MoveLinStruct& move_lin); - -private: - bool calculateRampProfile(std::vector* path_matrix, const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, cob_cartesian_controller::MoveLinStruct& move_lin); - bool calculateSinoidProfile(std::vector* path_matrix, const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, cob_cartesian_controller::MoveLinStruct& move_lin); - - bool generateRampPath(std::vector& path_array, double vel_max, double accel_max, const double Se_max, const int steps_max); - bool generateSinoidPath(std::vector& path_array, double vel_max, double accel_max, const double Se_max, const int steps_max); - - bool generateRampPathWithTe(std::vector& path_array, double te, double accl_max, const double Se_max, const int steps_max, const double start_angle); - bool generateSinoidPathWithTe(std::vector& path_array, double te, double accl_max, const double Se_max, const int steps_max, const double start_angle); - - double update_rate_; - double t_ipo_; -}; - -#endif /* COB_CARTESIAN_CONTROLLER_PROFILE_GENERATOR_H_ */ diff --git a/cob_cartesian_controller/msg/MoveCirc.msg b/cob_cartesian_controller/msg/MoveCirc.msg index 5c044bc7..1c4f2cda 100644 --- a/cob_cartesian_controller/msg/MoveCirc.msg +++ b/cob_cartesian_controller/msg/MoveCirc.msg @@ -6,5 +6,3 @@ float64 end_angle float64 radius bool rotate_only - -cob_cartesian_controller/Profile profile diff --git a/cob_cartesian_controller/msg/MoveLin.msg b/cob_cartesian_controller/msg/MoveLin.msg index 47c6bc73..1f626d5e 100644 --- a/cob_cartesian_controller/msg/MoveLin.msg +++ b/cob_cartesian_controller/msg/MoveLin.msg @@ -2,5 +2,3 @@ geometry_msgs/Pose pose_goal string frame_id bool rotate_only - -cob_cartesian_controller/Profile profile diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index 9ac8aa38..c322ed89 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -197,12 +197,12 @@ bool CartesianController::movePTP(const geometry_msgs::Pose& target_pose, const // Broadcasting interpolated Cartesian path bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& cartesian_path) { - bool success = false; + bool success = true; double epsilon = 0.1; int failure_counter = 0; ros::Rate rate(update_rate_); tf::Transform transform; - for(int i = 0; i < cartesian_path.poses.size()-1; i++) + for(int i = 0; i < cartesian_path.poses.size(); i++) { if(!as_->isActive()) { @@ -267,7 +267,7 @@ void CartesianController::goalCallback() ROS_INFO("move_lin"); // Interpolate path - if(!trajectory_interpolator_->linearInterpolation(cartesian_path, action_struct.move_lin)) + if(!trajectory_interpolator_->linearInterpolation(cartesian_path, action_struct)) { actionAbort(false, "Failed to do interpolation for 'move_lin'"); return; @@ -279,7 +279,7 @@ void CartesianController::goalCallback() // Activate Tracking if(!startTracking()) { - actionAbort(false, "Failed to start traccking"); + actionAbort(false, "Failed to start tracking"); return; } @@ -293,7 +293,7 @@ void CartesianController::goalCallback() // De-Activate Tracking if(!stopTracking()) { - actionAbort(false, "Failed to stop traccking"); + actionAbort(false, "Failed to stop tracking"); return; } @@ -303,7 +303,7 @@ void CartesianController::goalCallback() { ROS_INFO("move_circ"); - if(!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct.move_circ)) + if(!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct)) { actionAbort(false, "Failed to do interpolation for 'move_circ'"); return; @@ -315,7 +315,7 @@ void CartesianController::goalCallback() // Activate Tracking if(!startTracking()) { - actionAbort(false, "Failed to start traccking"); + actionAbort(false, "Failed to start tracking"); return; } @@ -336,7 +336,7 @@ void CartesianController::goalCallback() // De-Activate Tracking if(!stopTracking()) { - actionAbort(false, "Failed to stop traccking"); + actionAbort(false, "Failed to stop tracking"); return; } @@ -355,16 +355,13 @@ cob_cartesian_controller::MoveLinStruct CartesianController::convertMoveLin(cons start = utils_.getPose(root_frame_, chain_tip_link_); //current tcp pose utils_.transformPose(move_lin_msg.frame_id, root_frame_, move_lin_msg.pose_goal, end); - cob_cartesian_controller::MoveLinStruct move_lin_struct; - move_lin_struct.rotate_only = move_lin_msg.rotate_only; - move_lin_struct.profile.vel = move_lin_msg.profile.vel; - move_lin_struct.profile.accl = move_lin_msg.profile.accl; - move_lin_struct.profile.profile_type = move_lin_msg.profile.profile_type; + cob_cartesian_controller::MoveLinStruct move_lin; + move_lin.rotate_only = move_lin_msg.rotate_only; - move_lin_struct.start = start; - move_lin_struct.end = end; + move_lin.start = start; + move_lin.end = end; - return move_lin_struct; + return move_lin; } cob_cartesian_controller::MoveCircStruct CartesianController::convertMoveCirc(const cob_cartesian_controller::MoveCirc& move_circ_msg) @@ -372,17 +369,15 @@ cob_cartesian_controller::MoveCircStruct CartesianController::convertMoveCirc(co geometry_msgs::Pose center; utils_.transformPose(move_circ_msg.frame_id, root_frame_, move_circ_msg.pose_center, center); - cob_cartesian_controller::MoveCircStruct move_circ_struct; - move_circ_struct.start_angle = move_circ_msg.start_angle; - move_circ_struct.end_angle = move_circ_msg.end_angle; - move_circ_struct.radius = move_circ_msg.radius; - move_circ_struct.profile.vel = move_circ_msg.profile.vel; - move_circ_struct.profile.accl = move_circ_msg.profile.accl; - move_circ_struct.profile.profile_type = move_circ_msg.profile.profile_type; + cob_cartesian_controller::MoveCircStruct move_circ; + move_circ.start_angle = move_circ_msg.start_angle; + move_circ.end_angle = move_circ_msg.end_angle; + move_circ.radius = move_circ_msg.radius; + - move_circ_struct.pose_center = center; + move_circ.pose_center = center; - return move_circ_struct; + return move_circ; } cob_cartesian_controller::CartesianActionStruct CartesianController::acceptGoal(boost::shared_ptr goal) @@ -390,6 +385,12 @@ cob_cartesian_controller::CartesianActionStruct CartesianController::acceptGoal( cob_cartesian_controller::CartesianActionStruct action_struct; action_struct.move_type = goal->move_type; + action_struct.profile.vel = goal->profile.vel; + action_struct.profile.accl = goal->profile.accl; + action_struct.profile.profile_type = goal->profile.profile_type; + action_struct.profile.t_ipo = 1/update_rate_; + + if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::LIN) { action_struct.move_lin = convertMoveLin(goal->move_lin); @@ -400,6 +401,7 @@ cob_cartesian_controller::CartesianActionStruct CartesianController::acceptGoal( } else { + actionAbort(false, "Unknown trajectory action " + boost::lexical_cast(action_struct.move_type)); } diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 7c76a251..42e4d399 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -27,10 +27,14 @@ ****************************************************************/ #include +#include bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_array, - cob_cartesian_controller::MoveLinStruct& move_lin) + const cob_cartesian_controller::CartesianActionStruct& as) { + + this->trajectory_profile_generator_.reset(TrajectoryProfileBuilder::createProfile(as)); + pose_array.header.stamp = ros::Time::now(); pose_array.header.frame_id = root_frame_; @@ -42,15 +46,15 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ std::vector path_matrix[4]; geometry_msgs::Pose pose; - double Se = sqrt(pow((move_lin.end.position.x - move_lin.start.position.x), 2) + - pow((move_lin.end.position.y - move_lin.start.position.y), 2) + - pow((move_lin.end.position.z - move_lin.start.position.z), 2)); + double Se = sqrt(pow((as.move_lin.end.position.x - as.move_lin.start.position.x), 2) + + pow((as.move_lin.end.position.y - as.move_lin.start.position.y), 2) + + pow((as.move_lin.end.position.z - as.move_lin.start.position.z), 2)); // Convert Quaternions into RPY Angles for start and end pose - tf::quaternionMsgToTF(move_lin.start.orientation, q); + tf::quaternionMsgToTF(as.move_lin.start.orientation, q); tf::Matrix3x3(q).getRPY(start_roll, start_pitch, start_yaw); - tf::quaternionMsgToTF(move_lin.end.orientation, q); + tf::quaternionMsgToTF(as.move_lin.end.orientation, q); tf::Matrix3x3(q).getRPY(end_roll, end_pitch, end_yaw); // Calculate path length for the angular movement @@ -59,8 +63,10 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ Se_pitch = end_pitch - start_pitch; Se_yaw = end_yaw - start_yaw; + ROS_INFO_STREAM("Se: " << Se << " Se_roll: " << Se_roll << " Se_pitch: " << Se_pitch << " Se_yaw: " << Se_yaw); + // Calculate path for each Angle - if(!trajectory_profile_generator_lin_.calculateProfile(path_matrix, Se, Se_roll, Se_pitch, Se_yaw, move_lin)) + if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, Se_roll, Se_pitch, Se_yaw, as.move_lin.start)) { return false; } @@ -73,18 +79,9 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ // Interpolate the linear path for(int i = 0 ; i < path_matrix[0].size() ; i++) { - if(move_lin.rotate_only) - { - pose.position.x = move_lin.start.position.x; - pose.position.y = move_lin.start.position.y; - pose.position.z = move_lin.start.position.z; - } - else - { - pose.position.x = move_lin.start.position.x + linear_path.at(i) * (move_lin.end.position.x - move_lin.start.position.x) / Se; - pose.position.y = move_lin.start.position.y + linear_path.at(i) * (move_lin.end.position.y - move_lin.start.position.y) / Se; - pose.position.z = move_lin.start.position.z + linear_path.at(i) * (move_lin.end.position.z - move_lin.start.position.z) / Se; - } + pose.position.x = as.move_lin.start.position.x + linear_path.at(i) * (as.move_lin.end.position.x - as.move_lin.start.position.x) / Se; + pose.position.y = as.move_lin.start.position.y + linear_path.at(i) * (as.move_lin.end.position.y - as.move_lin.start.position.y) / Se; + pose.position.z = as.move_lin.start.position.z + linear_path.at(i) * (as.move_lin.end.position.z - as.move_lin.start.position.z) / Se; // Transform RPY to Quaternion q.setRPY(roll_path.at(i), pitch_path.at(i), yaw_path.at(i)); @@ -96,8 +93,10 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ } bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pose_array, - cob_cartesian_controller::MoveCircStruct& move_circ) + const cob_cartesian_controller::CartesianActionStruct& as) { + this->trajectory_profile_generator_.reset(TrajectoryProfileBuilder::createProfile(as)); + pose_array.header.stamp = ros::Time::now(); pose_array.header.frame_id = root_frame_; @@ -105,9 +104,11 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos tf::Transform C, P, T; std::vector path_array; + std::vector path_matrix[4]; + geometry_msgs::Pose pose; - double Se = move_circ.end_angle - move_circ.start_angle; + double Se = as.move_circ.end_angle - as.move_circ.start_angle; bool forward; if(Se < 0) @@ -121,30 +122,31 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos Se = std::fabs(Se); // Calculates the Path with RAMP or SINOID profile - if(!trajectory_profile_generator_circ_.calculateProfile(path_array, Se, move_circ.profile)) + if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0, 0, 0, pose)) { return false; } + path_array = path_matrix[0]; // Define Center Pose - C.setOrigin(tf::Vector3(move_circ.pose_center.position.x, move_circ.pose_center.position.y, move_circ.pose_center.position.z)); - tf::quaternionMsgToTF(move_circ.pose_center.orientation, q); + C.setOrigin(tf::Vector3(as.move_circ.pose_center.position.x, as.move_circ.pose_center.position.y, as.move_circ.pose_center.position.z)); + tf::quaternionMsgToTF(as.move_circ.pose_center.orientation, q); C.setRotation(q); // Interpolate the circular path for(int i = 0 ; i < path_array.size() ; i++) { // Rotate T - T.setOrigin(tf::Vector3(cos(path_array.at(i)) * move_circ.radius, 0, sin(path_array.at(i)) * move_circ.radius)); + T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); if(forward) { - T.setOrigin(tf::Vector3(cos(path_array.at(i)) * move_circ.radius, 0, sin(path_array.at(i)) * move_circ.radius)); + T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); q.setRPY(0, -path_array.at(i), 0); } else { - T.setOrigin(tf::Vector3(cos(move_circ.start_angle - path_array.at(i)) * move_circ.radius, 0, sin(move_circ.start_angle - path_array.at(i)) * move_circ.radius)); + T.setOrigin(tf::Vector3(cos(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius, 0, sin(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius)); q.setRPY(0, path_array.at(i), 0); } diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp new file mode 100644 index 00000000..c94f1d8f --- /dev/null +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp @@ -0,0 +1,397 @@ + +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_twist_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: June, 2015 + * + * \brief + * This module contains the implementation of all interface types. + * + ****************************************************************/ +#define RAMP_PROFILE 1 +#define SINOID_PROFILE 2 +#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h" + +/* BEGIN TrajectoryProfileBase *****************************************************************************************/ + +TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesian_controller::CartesianActionStruct& params) +{ + TrajectoryProfileBase* ib = NULL; + switch(params.profile.profile_type) + { + case RAMP_PROFILE: + ib = new TrajectoryProfileRamp(params); + break; + case SINOID_PROFILE: + ib = new TrajectoryProfileSinoid(params); + break; + default: + ROS_ERROR("Unknown Profile"); + break; + } + + return ib; +} +/* END TrajectoryProfileBase *******************************************************************************************/ + +/* BEGIN TrajectoryProfileRamp ********************************************************************************************/ +inline void TrajectoryProfileRamp::getProfileTimings(double Se_max, double accl, double vel) +{ + int steps_te, steps_tv, steps_tb = 0; + double tv, tb, te = 0.0; + + // Calculate the Ramp Profile Parameters + if (vel > sqrt(std::fabs(Se_max) * accl)) + { + vel = sqrt(std::fabs(Se_max) * accl); + } + tb = vel / accl; + te = (std::fabs(Se_max) / vel) + tb; + tv = te - tb; + + // Interpolationsteps for every timesequence + steps_tb = (double)tb / params_.profile.t_ipo; + steps_tv = (double)(tv-tb) / params_.profile.t_ipo; + steps_te = (double)(te-tv) / params_.profile.t_ipo; + + // Reconfigure timings wtih t_ipo_ + pt_.tb = steps_tb * params_.profile.t_ipo; + pt_.tv = (steps_tb + steps_tv) * params_.profile.t_ipo; + pt_.te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; + pt_.steps_tb = steps_tb; + pt_.steps_tv = steps_tv; + pt_.steps_te = steps_te; + pt_.max_steps = steps_tb + steps_tv + steps_te; +} + +inline void TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray &pa) +{ + std::vector array; + double accl = params_.profile.accl; + + if(pa.getCalcTe()) + { + int i = 0; + // Calculate the ramp profile path + // 0 <= t <= tb + for(; i <= pt_.steps_tb ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(0.5*accl*pow((params_.profile.t_ipo*i),2))); + } + + // tb <= t <= tv + for(; i <= (pt_.steps_tb + pt_.steps_tv) ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(params_.profile.vel*(params_.profile.t_ipo*i)-0.5*pow(params_.profile.vel,2)/accl)); + } + + // tv <= t <= te + for(; i <= (pt_.steps_tv + pt_.steps_tb + pt_.steps_te) ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(params_.profile.vel * (pt_.te-pt_.tb) - 0.5*accl* pow(pt_.te-(i*params_.profile.t_ipo),2))); + } + } + else + { + if(std::fabs(pa.getSe()) > 0.001) + { + double vel_max; + double accl_max = params_.profile.accl; + // Calculate the Profile Timings + // Reconfigure accl_max and Velmax + while(pt_.te < 2 * sqrt(std::fabs(pa.getSe())/accl_max)) + { + accl_max+=0.001; + } + vel_max = accl_max * pt_.te / 2 - sqrt((pow(accl_max,2)*pow(pt_.te,2)/4) - std::fabs(pa.getSe()) * accl_max ); + + // Calculate the ramp profile path + int i=0; + // 0 <= t <= tb + for(; i <= pt_.steps_tb ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(0.5*accl_max*pow((params_.profile.t_ipo*i),2))); + } + // tb <= t <= tv + for(;i<=(pt_.steps_tb + pt_.steps_tv);i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(vel_max*(params_.profile.t_ipo*i)-0.5*pow(vel_max,2)/accl_max)); + } + // tv <= t <= te + for(; i <= (pt_.steps_tv + pt_.steps_tb + pt_.steps_te);i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(vel_max * (pt_.te-pt_.tb) - 0.5*accl_max* pow(pt_.te-(i*params_.profile.t_ipo),2))); + } + } + else + { + array.push_back(0); + } + } + + + pa.setArray(array); + + + +} + +inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_matrix[4], + const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, + geometry_msgs::Pose start) +{ + double accl = params_.profile.accl; + double vel = params_.profile.vel; + + //Convert to RPY + double roll_start, pitch_start, yaw_start; + tf::Quaternion q; + tf::quaternionMsgToTF(start.orientation, q); + tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); + + std::vector linear_path, roll_path, pitch_path, yaw_path; + + cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); + cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); + cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); + cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); + cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); + + std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) + + getProfileTimings(sortedMatrix[0].getSe(), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + + // Calculate the paths + for(int i=0; i<4; i++) + { + this->generatePath(sortedMatrix[i]); + } + + // Resize the path vectors + for(int i=0; i < 4; i++) + { + this->pt_.max_steps = std::max((unsigned int)sortedMatrix[i].getArray().size() , this->pt_.max_steps); + } + + // Re-adjust the Matrix to its originally form. Index 0 to 3 + std::vector temp_array; + cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); + for(int j = 0; j<4; j++) + { + for(int i = 0; i<3; i++) + { + if(sortedMatrix[i].getIdx() > sortedMatrix[i+1].getIdx()) + { + temp = sortedMatrix[i]; + sortedMatrix[i] = sortedMatrix[i+1]; + sortedMatrix[i+1] = temp; + } + } + } + + path_matrix[0] = sortedMatrix[0].getArray(); + path_matrix[1] = sortedMatrix[1].getArray(); + path_matrix[2] = sortedMatrix[2].getArray(); + path_matrix[3] = sortedMatrix[3].getArray(); + + for(int i = 0; i < 4; i++) + { + if(path_matrix[i].size() < pt_.max_steps) + { + path_matrix[i].resize(pt_.max_steps, path_matrix[i].back()); + } + } + + return true; +} + + +/* END TrajectoryProfileRamp **********************************************************************************************/ + + + + + + + +/* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ +inline void TrajectoryProfileSinoid::getProfileTimings(double Se_max, double accl, double vel) +{ + int steps_te, steps_tv, steps_tb = 0; + double tv, tb, te = 0.0; + + // Calculate the Ramp Profile Parameters + if (vel > sqrt(std::fabs(Se_max) * accl / 2)) + { + vel = sqrt(std::fabs(Se_max) * accl / 2); + } + tb = 2 * vel / accl; + te = (std::fabs(Se_max) / vel) + tb; + tv = te - tb; + + // Interpolationsteps for every timesequence + steps_tb = (double)tb / params_.profile.t_ipo; + steps_tv = (double)(tv-tb) / params_.profile.t_ipo; + steps_te = (double)(te-tv) / params_.profile.t_ipo; + + // Reconfigure timings wtih t_ipo_ + pt_.tb = steps_tb * params_.profile.t_ipo; + pt_.tv = (steps_tb + steps_tv) * params_.profile.t_ipo; + pt_.te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; + pt_.steps_tb = steps_tb; + pt_.steps_tv = steps_tv; + pt_.steps_te = steps_te; + pt_.max_steps = steps_tb + steps_tv + steps_te; +} + +inline void TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) +{ + std::vector array; + double accl = params_.profile.accl; + + if(pa.getCalcTe()) + { + int i = 0; + // Calculate the sinoid profile path + for(; i <= pt_.steps_tb ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( params_.profile.accl*(0.25*pow(i*params_.profile.t_ipo,2) + pow(pt_.tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/pt_.tb * (i*params_.profile.t_ipo))-1)))); + } + // tb <= t <= tv + for(; i <= (pt_.steps_tb + pt_.steps_tv) ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( params_.profile.vel*(i*params_.profile.t_ipo-0.5*pt_.tb))); + } + // tv <= t <= te + for(; i < (pt_.steps_tv + pt_.steps_tb + pt_.steps_te) ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( 0.5 * params_.profile.accl *(pt_.te*(i*params_.profile.t_ipo + pt_.tb) - 0.5*(pow(i*params_.profile.t_ipo,2)+pow(pt_.te,2)+2*pow(pt_.tb,2)) + (pow(pt_.tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/pt_.tb) * (i*params_.profile.t_ipo-pt_.tv)))))); + } + } + else + { + if(std::fabs(pa.getSe()) > 0.001) + { + double vel_max; + double accl_max = params_.profile.accl; + // Calculate the Profile Timings + // Reconfigure accl_max and Velmax + while(pt_.te < sqrt(std::fabs(pa.getSe()) * 8/accl_max)) + { + accl_max += 0.001; + } + vel_max = accl_max * pt_.te / 4 - sqrt((pow(accl_max,2)*pow(pt_.te,2)/16) - std::fabs(pa.getSe()) * accl_max/2 ); + + int i = 0; + // Calculate the sinoid profile path + for(; i <= pt_.steps_tb ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(accl_max*(0.25*pow(i*params_.profile.t_ipo,2) + pow(pt_.tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/pt_.tb * (i*params_.profile.t_ipo))-1)))); + } + // tb <= t <= tv + for(; i <= (pt_.steps_tb + pt_.steps_tv) ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( vel_max*(i*params_.profile.t_ipo-0.5*pt_.tb))); + } + // tv <= t <= te + for(; i < (pt_.steps_tv + pt_.steps_tb + pt_.steps_te) ; i++) + { + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( 0.5 * accl_max *(pt_.te*(i*params_.profile.t_ipo + pt_.tb) - 0.5*(pow(i*params_.profile.t_ipo,2)+pow(pt_.te,2)+2*pow(pt_.tb,2)) + (pow(pt_.tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/pt_.tb) * (i*params_.profile.t_ipo-pt_.tv)))))); + } + } + else + { + array.push_back(0); + } + } + + pa.setArray(array); +} + +inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_matrix[4], + const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, + geometry_msgs::Pose start) +{ + double accl = params_.profile.accl; + double vel = params_.profile.vel; + + //Convert to RPY + double roll_start, pitch_start, yaw_start; + tf::Quaternion q; + tf::quaternionMsgToTF(start.orientation, q); + tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); + + std::vector linear_path, roll_path, pitch_path, yaw_path; + + cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); + cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); + cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); + cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); + cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); + + std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) + + getProfileTimings(sortedMatrix[0].getSe(), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + + // Calculate the paths + for(int i=0; i<4; i++) + { + this->generatePath(sortedMatrix[i]); + } + + // Resize the path vectors + for(int i=0; i < 4; i++) + { + this->pt_.max_steps = std::max((unsigned int)sortedMatrix[i].getArray().size() , this->pt_.max_steps); + } + + // Re-adjust the Matrix to its originally form. Index 0 to 3 + std::vector temp_array; + cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); + for(int j = 0; j<4; j++) + { + for(int i = 0; i<3; i++) + { + if(sortedMatrix[i].getIdx() > sortedMatrix[i+1].getIdx()) + { + temp = sortedMatrix[i]; + sortedMatrix[i] = sortedMatrix[i+1]; + sortedMatrix[i+1] = temp; + } + } + } + + path_matrix[0] = sortedMatrix[0].getArray(); + path_matrix[1] = sortedMatrix[1].getArray(); + path_matrix[2] = sortedMatrix[2].getArray(); + path_matrix[3] = sortedMatrix[3].getArray(); + + for(int i = 0; i < 4; i++) + { + if(path_matrix[i].size() < pt_.max_steps) + { + path_matrix[i].resize(pt_.max_steps, path_matrix[i].back()); + } + } + + return true; +} +/* END TrajectoryProfileSinoid ******************************************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_circ.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_circ.cpp deleted file mode 100644 index f4eec572..00000000 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_circ.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/*! - ***************************************************************** - * \file - * - * \note - * Copyright (c) 2015 \n - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) \n\n - * - ***************************************************************** - * - * \note - * Project name: care-o-bot - * \note - * ROS stack name: cob_control - * \note - * ROS package name: cob_cartesian_controller - * - * \author - * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com - * - * \date Date of creation: July, 2015 - * - * \brief - * ... - * - ****************************************************************/ - -#include -#include - - -bool TrajectoryProfileGeneratorCirc::calculateProfile(std::vector& path_array, const double Se, cob_cartesian_controller::ProfileStruct& profile) -{ - if(profile.profile_type == cob_cartesian_controller::Profile::RAMP) - { - ROS_INFO("Ramp Profile"); - return calculateRampProfile(path_array, Se, profile); - } - else if(profile.profile_type == cob_cartesian_controller::Profile::SINOID) - { - ROS_INFO("Sinoid Profile"); - return calculateSinoidProfile(path_array, Se, profile); - } - else - { - ROS_ERROR("Unknown Profile"); - return false; - } -} - -bool TrajectoryProfileGeneratorCirc::calculateRampProfile(std::vector& path_array, const double Se, cob_cartesian_controller::ProfileStruct& profile) -{ - double vel_max = profile.vel; - double accl_max = profile.accl; - int steps_te, steps_tv, steps_tb = 0; - double tv, tb, te = 0.0; - - // Calculate the Ramp Profile Parameters - if (vel_max > sqrt(Se*accl_max)) - { - vel_max = sqrt(Se*accl_max); - } - tb = vel_max/accl_max; - te = (Se / vel_max) + tb; - tv = te - tb; - -//ToDo: steps_tb is int! Do we need ceil() or floor()? - // Interpolationsteps for every timesequence - steps_tb = (double)tb / t_ipo_; - steps_tv = (double)(tv-tb) / t_ipo_; - steps_te = (double)(te-tv) / t_ipo_; - -//ToDo: What's the purpose of this? division by t_ipo_ followed by multiplication with t_ipo_? - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * t_ipo_; - tv = (steps_tb + steps_tv) * t_ipo_; - te = (steps_tb + steps_tv + steps_te) * t_ipo_; - - ROS_DEBUG("Vm: %f m/s", vel_max); - ROS_DEBUG("Bm: %f m/s^-2", accl_max); - ROS_DEBUG("Se: %f ", Se); - ROS_DEBUG("tb: %f s", tb); - ROS_DEBUG("tv: %f s", tv); - ROS_DEBUG("te: %f s", te); - -//ToDo: Verify for-conditions: 0 <= t < steps_tb.? - // Calculate the ramp profile path - // 0 <= t <= tb - for(int i = 0 ; i <= steps_tb-1 ; i++) - { - path_array.push_back(0.5 * accl_max * pow((t_ipo_ * i),2)); - } - // tb <= t <= tv - for(int i = steps_tb ; i <= (steps_tb + steps_tv-1) ; i++) - { - path_array.push_back(vel_max * (t_ipo_ * i) - 0.5 * pow(vel_max, 2)/accl_max); - } - // tv <= t <= te - for(int i = (steps_tb + steps_tv) ; i < (steps_tv + steps_tb + steps_te-1) ; i++) - { - path_array.push_back(vel_max * (te-tb) - 0.5 * accl_max * pow(te - (t_ipo_ * i), 2)); - } - - return true; -} - -bool TrajectoryProfileGeneratorCirc::calculateSinoidProfile(std::vector& path_array, const double Se, cob_cartesian_controller::ProfileStruct& profile) -{ - double vel_max = profile.vel; - double accl_max = profile.accl; - int steps_te, steps_tv, steps_tb = 0; - double tv, tb, te = 0.0; - - // Calculate the Sinoid Profile Parameters - if (vel_max > sqrt(Se*accl_max/2)) - { - vel_max = sqrt(Se*accl_max/2); - } - tb = 2 * vel_max/accl_max; - te = (Se / vel_max) + tb; - tv = te - tb; - -//ToDo: steps_tb is int! Do we need ceil() or floor()? - // Interpolationsteps for every timesequence - steps_tb = (double)tb / t_ipo_; - steps_tv = (double)(tv-tb) / t_ipo_; - steps_te = (double)(te-tv) / t_ipo_; - -//ToDo: What's the purpose of this? division by t_ipo_ followed by multiplication with t_ipo_? - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * t_ipo_; - tv = (steps_tb + steps_tv) * t_ipo_; - te = (steps_tb + steps_tv + steps_te) * t_ipo_; - - ROS_DEBUG("Vm: %f m/s", vel_max); - ROS_DEBUG("Bm: %f m/s^-2", accl_max); - ROS_DEBUG("Se: %f ", Se); - ROS_DEBUG("tb: %f s", tb); - ROS_DEBUG("tv: %f s", tv); - ROS_DEBUG("te: %f s", te); - -//ToDo: Verify for-conditions: 0 <= t < steps_tb.? - // Calculate the sinoid profile path - // 0 <= t <= tb - for(int i = 0 ; i <= steps_tb-1 ; i++) - { - path_array.push_back(accl_max * (0.25 * pow(t_ipo_ * i, 2) + pow(tb, 2)/(8 * pow(M_PI, 2)) * (cos(2 * M_PI/tb * (t_ipo_*i))-1))); - } - // tb <= t <= tv - for(int i = steps_tb ; i <= (steps_tb + steps_tv-1) ; i++) - { - path_array.push_back(vel_max * (i*t_ipo_ - 0.5 * tb)); - } - // tv <= t <= te - for(int i = (steps_tb + steps_tv) ; i < (steps_tv + steps_tb + steps_te-1) ; i++) - { - path_array.push_back(0.5 * accl_max * (te * (i*t_ipo_ + tb) - 0.5 * (pow(t_ipo_*i, 2) + pow(te, 2) + 2 * pow(tb, 2)) + (pow(tb, 2)/(4 * pow(M_PI, 2))) * (1 - cos( ((2*M_PI)/tb) * (i*t_ipo_ - tv))))); - } - - return true; -} diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_lin.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_lin.cpp deleted file mode 100644 index 222b6bd6..00000000 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_lin.cpp +++ /dev/null @@ -1,456 +0,0 @@ -/*! - ***************************************************************** - * \file - * - * \note - * Copyright (c) 2015 \n - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) \n\n - * - ***************************************************************** - * - * \note - * Project name: care-o-bot - * \note - * ROS stack name: cob_control - * \note - * ROS package name: cob_cartesian_controller - * - * \author - * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com - * - * \date Date of creation: July, 2015 - * - * \brief - * ... - * - ****************************************************************/ - -#include -#include - - -bool TrajectoryProfileGeneratorLin::calculateProfile(std::vector* path_matrix, - const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, - cob_cartesian_controller::MoveLinStruct& move_lin) -{ - if(move_lin.profile.profile_type == cob_cartesian_controller::Profile::RAMP) - { - ROS_INFO("Ramp Profile"); - return calculateRampProfile(path_matrix, Se, Se_roll, Se_pitch, Se_yaw, move_lin); - } - else if(move_lin.profile.profile_type == cob_cartesian_controller::Profile::SINOID) - { - ROS_INFO("Sinoid Profile"); - return calculateSinoidProfile(path_matrix, Se, Se_roll, Se_pitch, Se_yaw, move_lin); - } - else - { - ROS_ERROR("Unknown Profile"); - return false; - } -} - -bool TrajectoryProfileGeneratorLin::calculateRampProfile(std::vector* path_matrix, - const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, - cob_cartesian_controller::MoveLinStruct& move_lin) -{ - std::vector linear_path, roll_path, pitch_path, yaw_path; - int steps_te, steps_tv, steps_tb = 0; - double tv, tb, te = 0.0; - double Se_max = 0.0; - - // If rotateOnly == true, then set the largest angular difference as Se_max. - if(move_lin.rotate_only) - { - Se_max = std::max(std::fabs(Se), Se_max); - Se_max = std::max(std::fabs(Se_roll), Se_max); - Se_max = std::max(std::fabs(Se_pitch), Se_max); - Se_max = std::max(std::fabs(Se_yaw), Se_max); - } - else // Otherwise set the linear-path as Se_max - { - Se_max = Se; - } - - // Calculate the Ramp Profile Parameters - if (move_lin.profile.vel > sqrt(std::fabs(Se_max) * move_lin.profile.accl)) - { - move_lin.profile.vel = sqrt(std::fabs(Se_max) * move_lin.profile.accl); - } - tb = move_lin.profile.vel / move_lin.profile.accl; - te = (std::fabs(Se_max) / move_lin.profile.vel) + tb; - tv = te - tb; - -//ToDo: steps_tb is int! Do we need ceil() or floor()? - // Interpolationsteps for every timesequence - steps_tb = (double)tb / t_ipo_; - steps_tv = (double)(tv-tb) / t_ipo_; - steps_te = (double)(te-tv) / t_ipo_; - -//ToDo: What's the purpose of this? division by t_ipo_ followed by multiplication with t_ipo_? - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * t_ipo_; - tv = (steps_tb + steps_tv) * t_ipo_; - te = (steps_tb + steps_tv + steps_te) * t_ipo_; - - //Convert to RPY - double roll, pitch, yaw; - tf::Quaternion q; - tf::quaternionMsgToTF(move_lin.start.orientation, q); - tf::Matrix3x3(q).getRPY(roll, pitch, yaw); - - // Calculate the paths - if(!generateRampPath(linear_path, move_lin.profile.vel, move_lin.profile.accl, Se_max, (steps_tb + steps_tv + steps_te))) - { - ROS_WARN("Error while Calculating linear_path"); - return false; - } - if(!generateRampPathWithTe(roll_path, te, move_lin.profile.accl, Se_roll, (steps_tb + steps_tv + steps_te), roll)) - { - ROS_WARN("Error while Calculating roll_path"); - return false; - } - if(!generateRampPathWithTe(pitch_path, te, move_lin.profile.accl, Se_pitch, (steps_tb + steps_tv + steps_te), pitch)) - { - ROS_WARN("Error while Calculating pitch_path"); - return false; - } - if(!generateRampPathWithTe(yaw_path, te, move_lin.profile.accl, Se_yaw, (steps_tb + steps_tv + steps_te), yaw)) - { - ROS_WARN("Error while Calculating yaw_path"); - return false; - } - - // Resize the path vectors - unsigned int max_steps = 0; - max_steps = std::max((unsigned int)linear_path.size(), max_steps); - max_steps = std::max((unsigned int)roll_path.size(), max_steps); - max_steps = std::max((unsigned int)pitch_path.size(), max_steps); - max_steps = std::max((unsigned int)yaw_path.size(), max_steps); - - linear_path.resize(max_steps, linear_path.back()); - roll_path.resize(max_steps, roll_path.back()); - pitch_path.resize(max_steps, pitch_path.back()); - yaw_path.resize(max_steps, yaw_path.back()); - - // Put the interpolated paths into the path_matrix - path_matrix[LIN_INDEX] = linear_path; - path_matrix[ROLL_INDEX] = roll_path; - path_matrix[PITCH_INDEX] = pitch_path; - path_matrix[YAW_INDEX] = yaw_path; - - return true; -} - -bool TrajectoryProfileGeneratorLin::calculateSinoidProfile(std::vector* path_matrix, - const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, - cob_cartesian_controller::MoveLinStruct& move_lin) -{ - std::vector linear_path, roll_path, pitch_path, yaw_path; - int steps_te, steps_tv, steps_tb = 0; - double tv, tb, te = 0.0; - double Se_max = 0.0; - - // If rotateOnly == true, then set the largest angular difference as Se_max. - if(move_lin.rotate_only) - { - Se_max = std::max(std::fabs(Se), Se_max); - Se_max = std::max(std::fabs(Se_roll), Se_max); - Se_max = std::max(std::fabs(Se_pitch), Se_max); - Se_max = std::max(std::fabs(Se_yaw), Se_max); - } - else // Otherwise set the linear-path as Se_max - { - Se_max = Se; - } - - // Calculate the Sinoid Profile Parameters - if (move_lin.profile.vel > sqrt(std::fabs(Se_max) * move_lin.profile.accl / 2)) - { - move_lin.profile.vel = sqrt(std::fabs(Se_max) * move_lin.profile.accl / 2); - } - tb = 2 * move_lin.profile.vel / move_lin.profile.accl; - te = (std::fabs(Se_max) / move_lin.profile.vel) + tb; - tv = te - tb; - -//ToDo: steps_tb is int! Do we need ceil() or floor()? - // Interpolationsteps for every timesequence - steps_tb = (double)tb / t_ipo_; - steps_tv = (double)(tv-tb) / t_ipo_; - steps_te = (double)(te-tv) / t_ipo_; - -//ToDo: What's the purpose of this? division by t_ipo_ followed by multiplication with t_ipo_? - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * t_ipo_; - tv = (steps_tb + steps_tv) * t_ipo_; - te = (steps_tb + steps_tv + steps_te) * t_ipo_; - - //Convert to RPY - double roll, pitch, yaw; - tf::Quaternion q; - tf::quaternionMsgToTF(move_lin.start.orientation, q); - tf::Matrix3x3(q).getRPY(roll, pitch, yaw); - - // Calculate the paths - if(!generateSinoidPath(linear_path, move_lin.profile.vel, move_lin.profile.accl, Se_max, (steps_tb + steps_tv + steps_te))) - { - ROS_WARN("Error while Calculating linear_path"); - return false; - } - if(!generateSinoidPathWithTe(roll_path, te, move_lin.profile.accl, Se_roll, (steps_tb + steps_tv + steps_te), roll)) - { - ROS_WARN("Error while Calculating roll_path"); - return false; - } - if(!generateSinoidPathWithTe(pitch_path, te, move_lin.profile.accl, Se_pitch, (steps_tb + steps_tv + steps_te), pitch)) - { - ROS_WARN("Error while Calculating pitch_path"); - return false; - } - if(!generateSinoidPathWithTe(yaw_path, te, move_lin.profile.accl, Se_yaw, (steps_tb + steps_tv + steps_te), yaw)) - { - ROS_WARN("Error while Calculating yaw_path"); - return false; - } - - // Resize the path vectors - unsigned int max_steps = 0; - max_steps = std::max((unsigned int)linear_path.size(), max_steps); - max_steps = std::max((unsigned int)roll_path.size(), max_steps); - max_steps = std::max((unsigned int)pitch_path.size(), max_steps); - max_steps = std::max((unsigned int)yaw_path.size(), max_steps); - - linear_path.resize(max_steps, linear_path.back()); - roll_path.resize(max_steps, roll_path.back()); - pitch_path.resize(max_steps, pitch_path.back()); - yaw_path.resize(max_steps, yaw_path.back()); - - // Put the interpolated paths into the path_matrix - path_matrix[LIN_INDEX] = linear_path; - path_matrix[ROLL_INDEX] = roll_path; - path_matrix[PITCH_INDEX] = pitch_path; - path_matrix[YAW_INDEX] = yaw_path; - - return true; -} - -bool TrajectoryProfileGeneratorLin::generateRampPath(std::vector& path_array, double vel_max, double accl_max, const double Se_max, const int steps_max) -{ - double tv, tb, te = 0.0; - int steps_te, steps_tv, steps_tb = 0; - - // Reconfigure the timings and parameters with t_ipo_ - tb = (vel_max/(accl_max * t_ipo_)) * t_ipo_; - tv = (std::fabs(Se_max)/(vel_max * t_ipo_)) * t_ipo_; - te = tv + tb; - vel_max = std::fabs(Se_max) / tv; - accl_max = vel_max / tb; - - // Calculate the Profile Timings for the longest path - tb = vel_max/accl_max; - te = (std::fabs(Se_max) / vel_max) + tb; - tv = te - tb; - - // Interpolationsteps for every timesequence - steps_tb = (double)tb / t_ipo_; - steps_tv = (double)(tv-tb) / t_ipo_; - steps_te = (double)(te-tv) / t_ipo_; - - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * t_ipo_; - tv = (steps_tb + steps_tv) * t_ipo_; - te = (steps_tb + steps_tv + steps_te) * t_ipo_; - - ROS_DEBUG("Vm: %f m/s", vel_max); - ROS_DEBUG("Bm: %f m/s^-2", accl_max); - ROS_DEBUG("Se_max: %f ", Se_max); - ROS_DEBUG("tb: %f s", tb); - ROS_DEBUG("tv: %f s", tv); - ROS_DEBUG("te: %f s", te); - - // Calculate the ramp profile path - // 0 <= t <= tb - for(int i = 0 ; i <= steps_tb-1 ; i++) - { - path_array.push_back( Se_max/std::fabs(Se_max)*(0.5*accl_max*pow((t_ipo_*i),2))); - } - - // tb <= t <= tv - for(int i = steps_tb ; i <= (steps_tb + steps_tv-1) ; i++) - { - path_array.push_back(Se_max/std::fabs(Se_max)*(vel_max*(t_ipo_*i)-0.5*pow(vel_max,2)/accl_max)); - } - - // tv <= t <= te - for(int i = (steps_tb + steps_tv) ; i < (steps_tv + steps_tb + steps_te-1) ; i++) - { - path_array.push_back(Se_max/std::fabs(Se_max)*(vel_max * (te-tb) - 0.5*accl_max* pow(te-(i*t_ipo_),2))); - } - - return true; -} - -bool TrajectoryProfileGeneratorLin::generateSinoidPath(std::vector& path_array, double vel_max, double accl_max, const double Se_max, const int steps_max) -{ - double tv, tb, te = 0.0; - int steps_te, steps_tv, steps_tb = 0; - - // Reconfigure the timings and parameters with t_ipo_ - tb = (vel_max/(accl_max * t_ipo_)) * t_ipo_; - tv = (std::fabs(Se_max)/(vel_max * t_ipo_)) * t_ipo_; - te = tv + tb; - vel_max = std::fabs(Se_max) / tv; - accl_max = vel_max / tb; - - // Calculate the Profile Timings for the longest path - tb = 2 * vel_max/accl_max; - te = (std::fabs(Se_max) / vel_max) + tb; - tv = te - tb; - - // Interpolationsteps for every timesequence - steps_tb = (double)tb / t_ipo_; - steps_tv = (double)(tv-tb) / t_ipo_; - steps_te = (double)(te-tv) / t_ipo_; - - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * t_ipo_; - tv = (steps_tb + steps_tv) * t_ipo_; - te = (steps_tb + steps_tv + steps_te) * t_ipo_; - - ROS_DEBUG("Vm: %f m/s", vel_max); - ROS_DEBUG("Bm: %f m/s^-2", accl_max); - ROS_DEBUG("Se_max: %f ", Se_max); - ROS_DEBUG("tb: %f s", tb); - ROS_DEBUG("tv: %f s", tv); - ROS_DEBUG("te: %f s", te); - - // Calculate the sinoid profile path - // 0 <= t <= tb - for(int i = 0 ; i <= steps_tb-1 ; i++) - { - path_array.push_back( Se_max/std::fabs(Se_max)*( accl_max*(0.25*pow(i*t_ipo_,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*t_ipo_))-1)))); - } - // tb <= t <= tv - for(int i = steps_tb ; i <= (steps_tb + steps_tv-1) ; i++) - { - path_array.push_back(Se_max/std::fabs(Se_max)*( vel_max*(i*t_ipo_-0.5*tb))); - } - // tv <= t <= te - for(int i = (steps_tb + steps_tv) ; i < (steps_tv + steps_tb + steps_te-1) ; i++) - { - path_array.push_back(Se_max/std::fabs(Se_max)*( 0.5 * accl_max *( te*(i*t_ipo_ + tb) - 0.5*(pow(i*t_ipo_,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos( ((2*M_PI)/tb) * (i*t_ipo_-tv)))))); - } - - return true; -} - -bool TrajectoryProfileGeneratorLin::generateRampPathWithTe(std::vector& path_array, double te, - double accl_max, const double Se_max, const int steps_max, const double start_angle) -{ - double tv,tb = 0.0; - int steps_te, steps_tv, steps_tb = 0; - double vel_max; - - if(std::fabs(Se_max) > 0.001) - { - // Calculate the Profile Timings - // Reconfigure accl_max and Velmax - while(te < 2 * sqrt(std::fabs(Se_max)/accl_max)) - { - accl_max+=0.001; - } - - vel_max = accl_max * te / 2 - sqrt((pow(accl_max,2)*pow(te,2)/4) - std::fabs(Se_max) * accl_max ); - - // Calculate profile timings, te is known - tb = vel_max/accl_max; - tv = te - tb; - - // Interpolationsteps for every timesequence - steps_tb = (double)tb / t_ipo_; - steps_tv = (double)(tv-tb) / t_ipo_; - steps_te = (double)(te-tv) / t_ipo_; - - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * t_ipo_; - tv = (steps_tb + steps_tv) * t_ipo_; - te = (steps_tb + steps_tv + steps_te) * t_ipo_; - - // Calculate the ramp profile path - // 0 <= t <= tb - for(int i = 0 ; i <= steps_tb-1 ; i++) - { - path_array.push_back( start_angle + Se_max/std::fabs(Se_max)*(0.5*accl_max*pow((t_ipo_*i),2))); - } - // tb <= t <= tv - for(int i=steps_tb;i<=(steps_tb+steps_tv-1);i++) - { - path_array.push_back(start_angle + Se_max/std::fabs(Se_max)*(vel_max*(t_ipo_*i)-0.5*pow(vel_max,2)/accl_max)); - } - // tv <= t <= te - for(int i = (steps_tb + steps_tv) ; i < (steps_tv + steps_tb + steps_te-1);i++) - { - path_array.push_back(start_angle + Se_max/std::fabs(Se_max)*(vel_max * (te-tb) - 0.5*accl_max* pow(te-(i*t_ipo_),2))); - } - } - else - { - path_array.push_back(start_angle); - } - return true; -} - -bool TrajectoryProfileGeneratorLin::generateSinoidPathWithTe(std::vector& path_array, double te, - double accl_max, const double Se_max, const int steps_max, const double start_angle) -{ - double tv,tb = 0.0; - int steps_te, steps_tv, steps_tb = 0; - double vel_max; - - if(std::fabs(Se_max) > 0.001) - { - // Calculate the Profile Timings - // Reconfigure accl_max and Velmax - while(te < sqrt(std::fabs(Se_max) * 8/accl_max)) - { - accl_max += 0.001; - } - vel_max = accl_max * te / 4 - sqrt((pow(accl_max,2)*pow(te,2)/16) - std::fabs(Se_max) * accl_max/2 ); - - // Calculate profile timings, te is known - tb = 2 * vel_max/accl_max; - tv = te - tb; - - // Interpolationsteps for every timesequence - steps_tb = (double)tb / t_ipo_; - steps_tv = (double)(tv-tb) / t_ipo_; - steps_te = (double)(te-tv) / t_ipo_; - - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * t_ipo_; - tv = (steps_tb + steps_tv) * t_ipo_; - te = (steps_tb + steps_tv + steps_te) * t_ipo_; - - // Calculate the sinoid profile path - // 0 <= t <= tb - for(int i = 0 ; i <= steps_tb-1 ; i++) - { - path_array.push_back(start_angle + Se_max/std::fabs(Se_max)*( accl_max*(0.25*pow(i*t_ipo_,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*t_ipo_))-1)))); - } - // tb <= t <= tv - for(int i = steps_tb ; i <= (steps_tb + steps_tv-1) ; i++) - { - path_array.push_back(start_angle + Se_max/std::fabs(Se_max)*(vel_max*(i*t_ipo_-0.5*tb))); - } - // tv <= t <= te - for(int i = (steps_tb + steps_tv); i < (steps_tv+steps_tb+steps_te-1) ; i++){ - path_array.push_back(start_angle + Se_max/std::fabs(Se_max)*(0.5*accl_max*( te*(i*t_ipo_ + tb) - 0.5*(pow(i*t_ipo_,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos( ((2*M_PI)/tb) * (i*t_ipo_-tv)))))); - } - } - else - { - path_array.push_back(start_angle); - } - return true; -} From b1d7255e89ccd4fc8d9fb0e88ea0dc0af03b2589 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Thu, 1 Oct 2015 14:18:05 +0200 Subject: [PATCH 02/35] new example script --- .../scripts/example_move_lin.py | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100755 cob_cartesian_controller/scripts/example_move_lin.py diff --git a/cob_cartesian_controller/scripts/example_move_lin.py b/cob_cartesian_controller/scripts/example_move_lin.py new file mode 100755 index 00000000..63f88716 --- /dev/null +++ b/cob_cartesian_controller/scripts/example_move_lin.py @@ -0,0 +1,37 @@ +#! /usr/bin/env python +import rospy +import actionlib + +from cob_cartesian_controller.msg import CartesianControllerAction, CartesianControllerGoal +from cob_cartesian_controller.msg import Profile + +if __name__ == '__main__': + rospy.init_node('test_move_lin') + client = actionlib.SimpleActionClient('arm/cartesian_trajectory_action', CartesianControllerAction) + rospy.logwarn("Waiting for ActionServer...") + client.wait_for_server() + rospy.logwarn("...done") + + # Fill in the goal here + goal = CartesianControllerGoal() + + goal.move_type = CartesianControllerGoal.LIN + goal.move_lin.pose_goal.position.x = 0.3 + goal.move_lin.pose_goal.position.y = 0.3 + goal.move_lin.pose_goal.position.z = 0.5 + goal.move_lin.pose_goal.orientation.x = 0.0 + goal.move_lin.pose_goal.orientation.y = 0.0 + goal.move_lin.pose_goal.orientation.z = 0.0 + goal.move_lin.pose_goal.orientation.w = 1.0 + goal.move_lin.frame_id = 'world' + goal.move_lin.rotate_only = False + + goal.profile.vel = 0.05 + goal.profile.accl = 0.1 + goal.profile.profile_type = Profile.SINOID + + print goal + + # Send the goal + client.send_goal(goal) + client.wait_for_result() From ee50bbdb68d47928b4ec4eaa6e7e438c03f20529 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Fri, 2 Oct 2015 16:54:42 +0200 Subject: [PATCH 03/35] Fixed a bug in sort algorithm and profile interpolation --- .../cartesian_controller_data_types.h | 12 +- .../trajactory_profile_generator.h | 8 +- .../trajactory_profile_generator_base.h | 2 +- .../trajectory_profile_generator.cpp | 211 ++++++++++++------ 4 files changed, 156 insertions(+), 77 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 5f77b418..a9c881db 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -46,7 +46,7 @@ namespace cob_cartesian_controller struct ProfileTimings { double tb, te, tv; - unsigned int steps_tb, steps_te, steps_tv, max_steps; + unsigned int steps_tb, steps_te, steps_tv; }; struct MoveLinStruct @@ -154,11 +154,15 @@ namespace cob_cartesian_controller PathArray &pa2, PathArray &pa3, PathArray &pa4) +// PathArray &pa5, +// PathArray &pa6) { pm_.push_back(pa1); pm_.push_back(pa2); pm_.push_back(pa3); pm_.push_back(pa4); +// pm_.push_back(pa5); +// pm_.push_back(pa6); } ~PathMatrix() { @@ -183,11 +187,11 @@ namespace cob_cartesian_controller { std::vector tmp; PathArray temp(0, 0, 0.0, tmp); - for(int j = 0; j<4; j++) + for(int j = 0; j* path_matrix, double Se, double Se_roll, double Se_pitch, double Se_yaw, geometry_msgs::Pose start); - protected: + private: cob_cartesian_controller::ProfileTimings pt_; }; /* END TrajectoryProfileRamp **********************************************************************************************/ @@ -80,12 +80,12 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase ~TrajectoryProfileSinoid() {} virtual void getProfileTimings(double Se_max, double accl, double vel); - virtual void generatePath(cob_cartesian_controller::PathArray &pa); + virtual bool generatePath(cob_cartesian_controller::PathArray &pa); virtual bool calculateProfile(std::vector* path_matrix, double Se, double Se_roll, double Se_pitch, double Se_yaw, geometry_msgs::Pose start); - protected: + private: cob_cartesian_controller::ProfileTimings pt_; }; /* END TrajectoryProfileSinoid **********************************************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h index b1a2a188..b189784f 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h @@ -50,7 +50,7 @@ class TrajectoryProfileBase virtual void getProfileTimings(double Se_max, double accl, double vel) = 0; private: - virtual void generatePath(cob_cartesian_controller::PathArray &pa) = 0; + virtual bool generatePath(cob_cartesian_controller::PathArray &pa) = 0; protected: const cob_cartesian_controller::CartesianActionStruct ¶ms_; double vel_max_; diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp index c94f1d8f..41d82133 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp @@ -68,9 +68,9 @@ inline void TrajectoryProfileRamp::getProfileTimings(double Se_max, double accl, tv = te - tb; // Interpolationsteps for every timesequence - steps_tb = (double)tb / params_.profile.t_ipo; - steps_tv = (double)(tv-tb) / params_.profile.t_ipo; - steps_te = (double)(te-tv) / params_.profile.t_ipo; + steps_tb = trunc(tb / params_.profile.t_ipo); + steps_tv = trunc((tv-tb) / params_.profile.t_ipo); + steps_te = trunc((te-tv) / params_.profile.t_ipo); // Reconfigure timings wtih t_ipo_ pt_.tb = steps_tb * params_.profile.t_ipo; @@ -79,34 +79,37 @@ inline void TrajectoryProfileRamp::getProfileTimings(double Se_max, double accl, pt_.steps_tb = steps_tb; pt_.steps_tv = steps_tv; pt_.steps_te = steps_te; - pt_.max_steps = steps_tb + steps_tv + steps_te; } -inline void TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray &pa) +inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray &pa) { std::vector array; double accl = params_.profile.accl; + double direction = pa.getSe()/std::fabs(pa.getSe()); + if(pa.getCalcTe()) { - int i = 0; + ROS_INFO_STREAM("Longest Se.. Vel_max: " << params_.profile.vel << " accl_max: " << params_.profile.accl << " tb: " << pt_.tb << " tv: " << pt_.tv << " te: " << pt_.te); + + int i = 1; // Calculate the ramp profile path // 0 <= t <= tb for(; i <= pt_.steps_tb ; i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(0.5*accl*pow((params_.profile.t_ipo*i),2))); + array.push_back(pa.getStartValue() + direction * (0.5*params_.profile.accl*pow((params_.profile.t_ipo*i),2))); } // tb <= t <= tv for(; i <= (pt_.steps_tb + pt_.steps_tv) ; i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(params_.profile.vel*(params_.profile.t_ipo*i)-0.5*pow(params_.profile.vel,2)/accl)); + array.push_back(pa.getStartValue() + direction * (params_.profile.vel*(params_.profile.t_ipo*i) - 0.5 * pow(params_.profile.vel,2)/accl) ); } // tv <= t <= te - for(; i <= (pt_.steps_tv + pt_.steps_tb + pt_.steps_te) ; i++) + for(; i <= (pt_.steps_tv + pt_.steps_tb + pt_.steps_te + 1) ; i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(params_.profile.vel * (pt_.te-pt_.tb) - 0.5*accl* pow(pt_.te-(i*params_.profile.t_ipo),2))); + array.push_back(pa.getStartValue() + direction *(params_.profile.vel * (pt_.tv) - 0.5*accl* pow(pt_.te-(i*params_.profile.t_ipo),2))); } } else @@ -115,43 +118,74 @@ inline void TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathAr { double vel_max; double accl_max = params_.profile.accl; + double tb = pt_.tb; + double tv = pt_.tv; + double te = pt_.te; + + double steps_tb, steps_te, steps_tv; // Calculate the Profile Timings // Reconfigure accl_max and Velmax - while(pt_.te < 2 * sqrt(std::fabs(pa.getSe())/accl_max)) + + +// accl_max = std::fabs(pa.getSe()) / pow(0.5 * pt_.te, 2); + vel_max = accl_max * pt_.te / 2 - sqrt((pow(accl_max,2) * pow(pt_.te,2)/4) - std::fabs(pa.getSe()) * accl_max); + + tb = vel_max/accl_max; + tv = pt_.te - tb; + te = tv + tb; + + // Interpolationsteps for every timesequence + steps_tb = trunc(tb / params_.profile.t_ipo); + steps_tv = trunc((tv-tb) / params_.profile.t_ipo); + steps_te = trunc((te-tv) / params_.profile.t_ipo); + + // Reconfigure timings wtih t_ipo_ + tb = steps_tb * params_.profile.t_ipo; + tv = (steps_tb + steps_tv) * params_.profile.t_ipo; + te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; + + ROS_INFO_STREAM("Vel_max: " << vel_max << " accl_max: " << accl_max << " tb: " << tb << " tv: " << tv << " te: " << te); + + if(isnan(tb)!=0 || isnan(tv) !=0 || isnan(te) != 0 || isnan(accl_max) != 0 || isnan(vel_max) != 0) //NaN { - accl_max+=0.001; + ROS_WARN("Error while calculating the optimal velocity profile! \n Using the given parameters which may cause a longer operation time."); + tb = params_.profile.vel / params_.profile.accl; + tv = pt_.te - tb; + te = tv + tb; + steps_tb = trunc(tb / params_.profile.t_ipo); + steps_tv = trunc((tv-tb) / params_.profile.t_ipo); + steps_te = trunc((te-tv) / params_.profile.t_ipo); + accl_max = params_.profile.accl; + vel_max = params_.profile.vel; } - vel_max = accl_max * pt_.te / 2 - sqrt((pow(accl_max,2)*pow(pt_.te,2)/4) - std::fabs(pa.getSe()) * accl_max ); // Calculate the ramp profile path - int i=0; + int i = 1; // 0 <= t <= tb - for(; i <= pt_.steps_tb ; i++) + for(; i <= steps_tb ; i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(0.5*accl_max*pow((params_.profile.t_ipo*i),2))); + array.push_back(pa.getStartValue() + direction * (0.5*accl_max*pow((params_.profile.t_ipo*i),2))); } // tb <= t <= tv - for(;i<=(pt_.steps_tb + pt_.steps_tv);i++) + for(; i <= (steps_tb + steps_tv); i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(vel_max*(params_.profile.t_ipo*i)-0.5*pow(vel_max,2)/accl_max)); + array.push_back(pa.getStartValue() + direction *(vel_max*(params_.profile.t_ipo*i) - 0.5*pow(vel_max,2)/accl_max)); } // tv <= t <= te - for(; i <= (pt_.steps_tv + pt_.steps_tb + pt_.steps_te);i++) + for(; i <= (steps_tv + steps_tb + steps_te + 1); i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(vel_max * (pt_.te-pt_.tb) - 0.5*accl_max* pow(pt_.te-(i*params_.profile.t_ipo),2))); + array.push_back(pa.getStartValue() + direction * (vel_max * (te-tb) - 0.5*accl_max* pow(te-(i*params_.profile.t_ipo),2))); } } else { - array.push_back(0); + ROS_INFO_STREAM("Se = 0"); + + array.push_back(pa.getStartValue()); } } - - pa.setArray(array); - - - + return true; } inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_matrix[4], @@ -160,6 +194,7 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat { double accl = params_.profile.accl; double vel = params_.profile.vel; + unsigned int max_steps; //Convert to RPY double roll_start, pitch_start, yaw_start; @@ -177,26 +212,35 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) - getProfileTimings(sortedMatrix[0].getSe(), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + ROS_INFO("Got sorted_matrix."); + + this->getProfileTimings(std::fabs(sortedMatrix[0].getSe()), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + + ROS_INFO("Got profil timings."); // Calculate the paths - for(int i=0; i<4; i++) + for(int i=0; igeneratePath(sortedMatrix[i]); } + ROS_INFO("GeneratePath success."); + // Resize the path vectors - for(int i=0; i < 4; i++) + for(int i = 0; i < sortedMatrix.size(); i++) { - this->pt_.max_steps = std::max((unsigned int)sortedMatrix[i].getArray().size() , this->pt_.max_steps); + max_steps = std::max((unsigned int)sortedMatrix[i].getArray().size() , max_steps); } + ROS_INFO("max_steps success."); + // Re-adjust the Matrix to its originally form. Index 0 to 3 std::vector temp_array; cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); - for(int j = 0; j<4; j++) + + for(int j = 0; j < sortedMatrix.size(); j++) { - for(int i = 0; i<3; i++) + for(int i = 0; i < sortedMatrix.size()-1; i++) { if(sortedMatrix[i].getIdx() > sortedMatrix[i+1].getIdx()) { @@ -207,19 +251,24 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat } } - path_matrix[0] = sortedMatrix[0].getArray(); - path_matrix[1] = sortedMatrix[1].getArray(); - path_matrix[2] = sortedMatrix[2].getArray(); - path_matrix[3] = sortedMatrix[3].getArray(); + ROS_INFO("IndexSort success."); - for(int i = 0; i < 4; i++) + for(int i = 0; i < sortedMatrix.size(); i++) { - if(path_matrix[i].size() < pt_.max_steps) + path_matrix[i] = sortedMatrix[i].getArray(); + } + + ROS_INFO("Fill path_matrix success."); + + for(int i = 0; i < sortedMatrix.size(); i++) + { + if(path_matrix[i].size() < max_steps) { - path_matrix[i].resize(pt_.max_steps, path_matrix[i].back()); + path_matrix[i].resize(max_steps, path_matrix[i].back()); } } + ROS_INFO("Readjust size success."); return true; } @@ -259,17 +308,18 @@ inline void TrajectoryProfileSinoid::getProfileTimings(double Se_max, double acc pt_.steps_tb = steps_tb; pt_.steps_tv = steps_tv; pt_.steps_te = steps_te; - pt_.max_steps = steps_tb + steps_tv + steps_te; } -inline void TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) +inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) { std::vector array; double accl = params_.profile.accl; if(pa.getCalcTe()) { - int i = 0; + ROS_INFO_STREAM("Longest Se.. Vel_max: " << params_.profile.vel << " accl_max: " << params_.profile.accl << " tb: " << pt_.tb << " tv: " << pt_.tv << " te: " << pt_.te); + + int i = 1; // Calculate the sinoid profile path for(; i <= pt_.steps_tb ; i++) { @@ -281,7 +331,7 @@ inline void TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::Path array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( params_.profile.vel*(i*params_.profile.t_ipo-0.5*pt_.tb))); } // tv <= t <= te - for(; i < (pt_.steps_tv + pt_.steps_tb + pt_.steps_te) ; i++) + for(; i <= (pt_.steps_tv + pt_.steps_tb + pt_.steps_te + 1) ; i++) { array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( 0.5 * params_.profile.accl *(pt_.te*(i*params_.profile.t_ipo + pt_.tb) - 0.5*(pow(i*params_.profile.t_ipo,2)+pow(pt_.te,2)+2*pow(pt_.tb,2)) + (pow(pt_.tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/pt_.tb) * (i*params_.profile.t_ipo-pt_.tv)))))); } @@ -292,38 +342,64 @@ inline void TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::Path { double vel_max; double accl_max = params_.profile.accl; + double tb,te,tv; + double steps_tb, steps_te, steps_tv; + // Calculate the Profile Timings // Reconfigure accl_max and Velmax - while(pt_.te < sqrt(std::fabs(pa.getSe()) * 8/accl_max)) + + accl_max = 8 * std::fabs(pa.getSe())/pow(pt_.te, 2); + vel_max = accl_max * pt_.te / 4 - sqrt((pow(accl_max,2)*pow(pt_.te,2)/16) - std::fabs(pa.getSe()) * accl_max/2 ); + + tb = 2 * vel_max / accl_max; + tv = pt_.te - tb; + te = tv + tb; + + // Interpolationsteps for every timesequence + steps_tb = trunc(tb / params_.profile.t_ipo); + steps_tv = trunc((tv-tb) / params_.profile.t_ipo); + steps_te = trunc((te-tv) / params_.profile.t_ipo); + + if(isnan(tb)!=0 || isnan(tv) !=0 || isnan(te) != 0 || isnan(accl_max) != 0 || isnan(vel_max) != 0) //NaN { - accl_max += 0.001; + ROS_WARN("Error while calculating the optimal velocity profile! \n Using the given parameters which may cause a longer operation time."); + tb = 2 * params_.profile.vel / params_.profile.accl; + tv = pt_.te - tb; + te = tv + tb; + steps_tb = trunc(tb / params_.profile.t_ipo); + steps_tv = trunc((tv-tb) / params_.profile.t_ipo); + steps_te = trunc((te-tv) / params_.profile.t_ipo); + accl_max = params_.profile.accl; + vel_max = params_.profile.vel; } - vel_max = accl_max * pt_.te / 4 - sqrt((pow(accl_max,2)*pow(pt_.te,2)/16) - std::fabs(pa.getSe()) * accl_max/2 ); - int i = 0; + ROS_INFO_STREAM("Vel_max: " << vel_max << " accl_max: " << accl_max << " tb: " << tb << " tv: " << tv << " te: " << te); + int i = 1; // Calculate the sinoid profile path - for(; i <= pt_.steps_tb ; i++) + for(; i <= steps_tb ; i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(accl_max*(0.25*pow(i*params_.profile.t_ipo,2) + pow(pt_.tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/pt_.tb * (i*params_.profile.t_ipo))-1)))); + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(accl_max*(0.25*pow(i*params_.profile.t_ipo,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*params_.profile.t_ipo))-1)))); } // tb <= t <= tv - for(; i <= (pt_.steps_tb + pt_.steps_tv) ; i++) + for(; i <= (steps_tb + steps_tv) ; i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( vel_max*(i*params_.profile.t_ipo-0.5*pt_.tb))); + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( vel_max*(i*params_.profile.t_ipo-0.5*tb))); } // tv <= t <= te - for(; i < (pt_.steps_tv + pt_.steps_tb + pt_.steps_te) ; i++) + for(; i <= (steps_tv + steps_tb + steps_te + 1) ; i++) { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( 0.5 * accl_max *(pt_.te*(i*params_.profile.t_ipo + pt_.tb) - 0.5*(pow(i*params_.profile.t_ipo,2)+pow(pt_.te,2)+2*pow(pt_.tb,2)) + (pow(pt_.tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/pt_.tb) * (i*params_.profile.t_ipo-pt_.tv)))))); + array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( 0.5 * accl_max *(te*(i*params_.profile.t_ipo + tb) - 0.5*(pow(i*params_.profile.t_ipo,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/tb) * (i*params_.profile.t_ipo-tv)))))); } } else { + ROS_INFO_STREAM("Se = 0"); array.push_back(0); } } pa.setArray(array); + return true; } inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_matrix[4], @@ -332,6 +408,7 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m { double accl = params_.profile.accl; double vel = params_.profile.vel; + unsigned int max_steps; //Convert to RPY double roll_start, pitch_start, yaw_start; @@ -348,27 +425,26 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) - - getProfileTimings(sortedMatrix[0].getSe(), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + this->getProfileTimings(std::fabs(sortedMatrix[0].getSe()), accl, vel); // Calculate the velocity profile timings with respect to the largest Se // Calculate the paths - for(int i=0; i<4; i++) + for(int i=0; igeneratePath(sortedMatrix[i]); } // Resize the path vectors - for(int i=0; i < 4; i++) + for(int i = 0; i < sortedMatrix.size(); i++) { - this->pt_.max_steps = std::max((unsigned int)sortedMatrix[i].getArray().size() , this->pt_.max_steps); + max_steps = std::max((unsigned int)sortedMatrix[i].getArray().size() , max_steps); } // Re-adjust the Matrix to its originally form. Index 0 to 3 std::vector temp_array; cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); - for(int j = 0; j<4; j++) + for(int j = 0; j sortedMatrix[i+1].getIdx()) { @@ -379,19 +455,18 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m } } - path_matrix[0] = sortedMatrix[0].getArray(); - path_matrix[1] = sortedMatrix[1].getArray(); - path_matrix[2] = sortedMatrix[2].getArray(); - path_matrix[3] = sortedMatrix[3].getArray(); + for(int i = 0; i < sortedMatrix.size(); i++) + { + path_matrix[i] = sortedMatrix[i].getArray(); + } - for(int i = 0; i < 4; i++) + for(int i = 0; i < sortedMatrix.size(); i++) { - if(path_matrix[i].size() < pt_.max_steps) + if(path_matrix[i].size() < max_steps) { - path_matrix[i].resize(pt_.max_steps, path_matrix[i].back()); + path_matrix[i].resize(max_steps, path_matrix[i].back()); } } - return true; } /* END TrajectoryProfileSinoid ******************************************************************************************/ From 4392f970bf2bfe21cd1659a994d0a78630484a35 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 20 Oct 2015 18:21:55 +0200 Subject: [PATCH 04/35] Code reduction part 1.. there's still a bug in ramp profiles. --- cob_cartesian_controller/CMakeLists.txt | 2 +- .../cartesian_controller_data_types.h | 75 +--- .../trajactory_profile_generator.h | 50 +-- .../trajactory_profile_generator_base.h | 7 +- .../src/cartesian_controller.cpp | 16 +- .../trajectory_interpolator.cpp | 2 - .../trajectory_profile_generator.cpp | 421 +----------------- 7 files changed, 35 insertions(+), 538 deletions(-) diff --git a/cob_cartesian_controller/CMakeLists.txt b/cob_cartesian_controller/CMakeLists.txt index ef437283..82a5f455 100644 --- a/cob_cartesian_controller/CMakeLists.txt +++ b/cob_cartesian_controller/CMakeLists.txt @@ -38,7 +38,7 @@ add_library(cartesian_controller_utils src/cartesian_controller_utils.cpp) add_dependencies(cartesian_controller_utils ${catkin_EXPORTED_TARGETS}) target_link_libraries(cartesian_controller_utils ${catkin_LIBRARIES}) -add_library(profile_generator src/trajectory_profile_generator/trajectory_profile_generator.cpp) +add_library(profile_generator src/trajectory_profile_generator/trajectory_profile_generator.cpp src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp) add_dependencies(profile_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(profile_generator ${catkin_LIBRARIES}) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index a9c881db..55e1bbd3 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -81,7 +81,7 @@ namespace cob_cartesian_controller start_value_(start_value) { idx_= idx; - calcTe_=false; + calcTe_ = false; } ~PathArray() @@ -89,16 +89,6 @@ namespace cob_cartesian_controller array_.clear(); } - unsigned int getIdx(); - bool getCalcTe(); - double getSe(); - std::vector getArray(); - void setCalcTe(bool calcTe); - void setIdx(unsigned int idx); - void setArray(std::vector array); - double getStartValue(); - - private: unsigned int idx_; bool calcTe_; double Se_; @@ -107,62 +97,18 @@ namespace cob_cartesian_controller }; - inline double PathArray::getStartValue() - { - return start_value_; - } - - inline void PathArray::setArray(std::vector array) - { - array_ = array; - } - - inline void PathArray::setCalcTe(bool calcTe) - { - calcTe_ = calcTe; - } - - inline void PathArray::setIdx(unsigned int idx) - { - idx_=idx; - } - - inline unsigned int PathArray::getIdx() - { - return idx_; - } - - inline bool PathArray::getCalcTe() - { - return calcTe_; - } - - inline double PathArray::getSe() - { - return Se_; - } - - inline std::vector PathArray::getArray() - { - return array_; - } - - class PathMatrix{ public: PathMatrix(PathArray &pa1, PathArray &pa2, PathArray &pa3, PathArray &pa4) -// PathArray &pa5, -// PathArray &pa6) { pm_.push_back(pa1); pm_.push_back(pa2); pm_.push_back(pa3); pm_.push_back(pa4); -// pm_.push_back(pa5); -// pm_.push_back(pa6); + } ~PathMatrix() { @@ -191,7 +137,7 @@ namespace cob_cartesian_controller { for(int i = 0; i pm_[i+1].idx_) +// { +// temp = pm_[i]; +// pm_[i] = pm_[i+1]; +// pm_[i+1] = temp; +// } +// } +// } + + pm_[0].calcTe_ = true; // Reference Time for profile interpolation (Te) } diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h index be794619..6478a458 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h @@ -27,8 +27,8 @@ #ifndef TRAJECTORY_PROFILE_BUILDER_H_ #define TRAJECTORY_PROFILE_BUILDER_H_ -#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h" #include "cob_cartesian_controller/cartesian_controller_data_types.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h" /* BEGIN TrajectoryProfileBuilder *****************************************************************************************/ @@ -42,52 +42,4 @@ class TrajectoryProfileBuilder }; /* END TrajectoryGeneratorBuilder *******************************************************************************************/ - -/* BEGIN TrajectoryProfileRamp ****************************************************************************************/ -/// Class providing a HardwareInterface publishing velocities. -class TrajectoryProfileRamp: public TrajectoryProfileBase -{ - public: - TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) - : TrajectoryProfileBase(params) - { } - - ~TrajectoryProfileRamp() {} - - virtual void getProfileTimings(double Se_max, double accl, double vel); - virtual bool generatePath(cob_cartesian_controller::PathArray &pa); - virtual bool calculateProfile(std::vector* path_matrix, - double Se, double Se_roll, double Se_pitch, double Se_yaw, - geometry_msgs::Pose start); - - - - private: - cob_cartesian_controller::ProfileTimings pt_; -}; -/* END TrajectoryProfileRamp **********************************************************************************************/ - - -/* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ -/// Class providing a HardwareInterface publishing positions. -class TrajectoryProfileSinoid : public TrajectoryProfileBase -{ - public: - TrajectoryProfileSinoid(const cob_cartesian_controller::CartesianActionStruct& params) - : TrajectoryProfileBase(params) - { } - - ~TrajectoryProfileSinoid() {} - - virtual void getProfileTimings(double Se_max, double accl, double vel); - virtual bool generatePath(cob_cartesian_controller::PathArray &pa); - virtual bool calculateProfile(std::vector* path_matrix, - double Se, double Se_roll, double Se_pitch, double Se_yaw, - geometry_msgs::Pose start); - - private: - cob_cartesian_controller::ProfileTimings pt_; -}; -/* END TrajectoryProfileSinoid **********************************************************************************************/ - #endif diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h index b189784f..bdff75b5 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h @@ -47,10 +47,13 @@ class TrajectoryProfileBase virtual bool calculateProfile(std::vector path_matrix[4], double Se, double Se_roll, double Se_pitch, double Se_yaw, geometry_msgs::Pose start) = 0; - virtual void getProfileTimings(double Se_max, double accl, double vel) = 0; - + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel) = 0; + virtual std::vector getTrajectory(double start_value, double se, + double accl, double vel, double t_ipo, + double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; private: virtual bool generatePath(cob_cartesian_controller::PathArray &pa) = 0; + protected: const cob_cartesian_controller::CartesianActionStruct ¶ms_; double vel_max_; diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index c322ed89..711d7f87 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -202,7 +202,7 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca int failure_counter = 0; ros::Rate rate(update_rate_); tf::Transform transform; - for(int i = 0; i < cartesian_path.poses.size(); i++) + for(unsigned int i = 0; i < cartesian_path.poses.size(); i++) { if(!as_->isActive()) { @@ -212,12 +212,12 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca // Send/Refresh target Frame transform.setOrigin( tf::Vector3(cartesian_path.poses.at(i).position.x, - cartesian_path.poses.at(i).position.y, - cartesian_path.poses.at(i).position.z) ); + cartesian_path.poses.at(i).position.y, + cartesian_path.poses.at(i).position.z) ); transform.setRotation( tf::Quaternion(cartesian_path.poses.at(i).orientation.x, - cartesian_path.poses.at(i).orientation.y, - cartesian_path.poses.at(i).orientation.z, - cartesian_path.poses.at(i).orientation.w) ); + cartesian_path.poses.at(i).orientation.y, + cartesian_path.poses.at(i).orientation.z, + cartesian_path.poses.at(i).orientation.w) ); tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); // Get transformation @@ -244,7 +244,6 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca success = false; break; } - ros::spinOnce(); rate.sleep(); } @@ -258,7 +257,10 @@ void CartesianController::goalCallback() geometry_msgs::PoseArray cartesian_path; cob_cartesian_controller::CartesianActionStruct action_struct; + + ROS_INFO_STREAM("========================================================================="); ROS_INFO("Received a new goal"); + ROS_INFO_STREAM("========================================================================="); action_struct = acceptGoal(as_->acceptNewGoal()); diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 42e4d399..e6acd9b8 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -63,8 +63,6 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ Se_pitch = end_pitch - start_pitch; Se_yaw = end_yaw - start_yaw; - ROS_INFO_STREAM("Se: " << Se << " Se_roll: " << Se_roll << " Se_pitch: " << Se_pitch << " Se_yaw: " << Se_yaw); - // Calculate path for each Angle if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, Se_roll, Se_pitch, Se_yaw, as.move_lin.start)) { diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp index 41d82133..98b3f54d 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp @@ -29,6 +29,8 @@ #define RAMP_PROFILE 1 #define SINOID_PROFILE 2 #include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h" /* BEGIN TrajectoryProfileBase *****************************************************************************************/ @@ -51,422 +53,3 @@ TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesi return ib; } /* END TrajectoryProfileBase *******************************************************************************************/ - -/* BEGIN TrajectoryProfileRamp ********************************************************************************************/ -inline void TrajectoryProfileRamp::getProfileTimings(double Se_max, double accl, double vel) -{ - int steps_te, steps_tv, steps_tb = 0; - double tv, tb, te = 0.0; - - // Calculate the Ramp Profile Parameters - if (vel > sqrt(std::fabs(Se_max) * accl)) - { - vel = sqrt(std::fabs(Se_max) * accl); - } - tb = vel / accl; - te = (std::fabs(Se_max) / vel) + tb; - tv = te - tb; - - // Interpolationsteps for every timesequence - steps_tb = trunc(tb / params_.profile.t_ipo); - steps_tv = trunc((tv-tb) / params_.profile.t_ipo); - steps_te = trunc((te-tv) / params_.profile.t_ipo); - - // Reconfigure timings wtih t_ipo_ - pt_.tb = steps_tb * params_.profile.t_ipo; - pt_.tv = (steps_tb + steps_tv) * params_.profile.t_ipo; - pt_.te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; - pt_.steps_tb = steps_tb; - pt_.steps_tv = steps_tv; - pt_.steps_te = steps_te; -} - -inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray &pa) -{ - std::vector array; - double accl = params_.profile.accl; - - double direction = pa.getSe()/std::fabs(pa.getSe()); - - if(pa.getCalcTe()) - { - ROS_INFO_STREAM("Longest Se.. Vel_max: " << params_.profile.vel << " accl_max: " << params_.profile.accl << " tb: " << pt_.tb << " tv: " << pt_.tv << " te: " << pt_.te); - - int i = 1; - // Calculate the ramp profile path - // 0 <= t <= tb - for(; i <= pt_.steps_tb ; i++) - { - array.push_back(pa.getStartValue() + direction * (0.5*params_.profile.accl*pow((params_.profile.t_ipo*i),2))); - } - - // tb <= t <= tv - for(; i <= (pt_.steps_tb + pt_.steps_tv) ; i++) - { - array.push_back(pa.getStartValue() + direction * (params_.profile.vel*(params_.profile.t_ipo*i) - 0.5 * pow(params_.profile.vel,2)/accl) ); - } - - // tv <= t <= te - for(; i <= (pt_.steps_tv + pt_.steps_tb + pt_.steps_te + 1) ; i++) - { - array.push_back(pa.getStartValue() + direction *(params_.profile.vel * (pt_.tv) - 0.5*accl* pow(pt_.te-(i*params_.profile.t_ipo),2))); - } - } - else - { - if(std::fabs(pa.getSe()) > 0.001) - { - double vel_max; - double accl_max = params_.profile.accl; - double tb = pt_.tb; - double tv = pt_.tv; - double te = pt_.te; - - double steps_tb, steps_te, steps_tv; - // Calculate the Profile Timings - // Reconfigure accl_max and Velmax - - -// accl_max = std::fabs(pa.getSe()) / pow(0.5 * pt_.te, 2); - vel_max = accl_max * pt_.te / 2 - sqrt((pow(accl_max,2) * pow(pt_.te,2)/4) - std::fabs(pa.getSe()) * accl_max); - - tb = vel_max/accl_max; - tv = pt_.te - tb; - te = tv + tb; - - // Interpolationsteps for every timesequence - steps_tb = trunc(tb / params_.profile.t_ipo); - steps_tv = trunc((tv-tb) / params_.profile.t_ipo); - steps_te = trunc((te-tv) / params_.profile.t_ipo); - - // Reconfigure timings wtih t_ipo_ - tb = steps_tb * params_.profile.t_ipo; - tv = (steps_tb + steps_tv) * params_.profile.t_ipo; - te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; - - ROS_INFO_STREAM("Vel_max: " << vel_max << " accl_max: " << accl_max << " tb: " << tb << " tv: " << tv << " te: " << te); - - if(isnan(tb)!=0 || isnan(tv) !=0 || isnan(te) != 0 || isnan(accl_max) != 0 || isnan(vel_max) != 0) //NaN - { - ROS_WARN("Error while calculating the optimal velocity profile! \n Using the given parameters which may cause a longer operation time."); - tb = params_.profile.vel / params_.profile.accl; - tv = pt_.te - tb; - te = tv + tb; - steps_tb = trunc(tb / params_.profile.t_ipo); - steps_tv = trunc((tv-tb) / params_.profile.t_ipo); - steps_te = trunc((te-tv) / params_.profile.t_ipo); - accl_max = params_.profile.accl; - vel_max = params_.profile.vel; - } - - // Calculate the ramp profile path - int i = 1; - // 0 <= t <= tb - for(; i <= steps_tb ; i++) - { - array.push_back(pa.getStartValue() + direction * (0.5*accl_max*pow((params_.profile.t_ipo*i),2))); - } - // tb <= t <= tv - for(; i <= (steps_tb + steps_tv); i++) - { - array.push_back(pa.getStartValue() + direction *(vel_max*(params_.profile.t_ipo*i) - 0.5*pow(vel_max,2)/accl_max)); - } - // tv <= t <= te - for(; i <= (steps_tv + steps_tb + steps_te + 1); i++) - { - array.push_back(pa.getStartValue() + direction * (vel_max * (te-tb) - 0.5*accl_max* pow(te-(i*params_.profile.t_ipo),2))); - } - } - else - { - ROS_INFO_STREAM("Se = 0"); - - array.push_back(pa.getStartValue()); - } - } - pa.setArray(array); - return true; -} - -inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_matrix[4], - const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, - geometry_msgs::Pose start) -{ - double accl = params_.profile.accl; - double vel = params_.profile.vel; - unsigned int max_steps; - - //Convert to RPY - double roll_start, pitch_start, yaw_start; - tf::Quaternion q; - tf::quaternionMsgToTF(start.orientation, q); - tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); - - std::vector linear_path, roll_path, pitch_path, yaw_path; - - cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); - cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); - cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); - cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); - cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); - - std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) - - ROS_INFO("Got sorted_matrix."); - - this->getProfileTimings(std::fabs(sortedMatrix[0].getSe()), accl, vel); // Calculate the velocity profile timings with respect to the largest Se - - ROS_INFO("Got profil timings."); - - // Calculate the paths - for(int i=0; igeneratePath(sortedMatrix[i]); - } - - ROS_INFO("GeneratePath success."); - - // Resize the path vectors - for(int i = 0; i < sortedMatrix.size(); i++) - { - max_steps = std::max((unsigned int)sortedMatrix[i].getArray().size() , max_steps); - } - - ROS_INFO("max_steps success."); - - // Re-adjust the Matrix to its originally form. Index 0 to 3 - std::vector temp_array; - cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); - - for(int j = 0; j < sortedMatrix.size(); j++) - { - for(int i = 0; i < sortedMatrix.size()-1; i++) - { - if(sortedMatrix[i].getIdx() > sortedMatrix[i+1].getIdx()) - { - temp = sortedMatrix[i]; - sortedMatrix[i] = sortedMatrix[i+1]; - sortedMatrix[i+1] = temp; - } - } - } - - ROS_INFO("IndexSort success."); - - for(int i = 0; i < sortedMatrix.size(); i++) - { - path_matrix[i] = sortedMatrix[i].getArray(); - } - - ROS_INFO("Fill path_matrix success."); - - for(int i = 0; i < sortedMatrix.size(); i++) - { - if(path_matrix[i].size() < max_steps) - { - path_matrix[i].resize(max_steps, path_matrix[i].back()); - } - } - - ROS_INFO("Readjust size success."); - return true; -} - - -/* END TrajectoryProfileRamp **********************************************************************************************/ - - - - - - - -/* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ -inline void TrajectoryProfileSinoid::getProfileTimings(double Se_max, double accl, double vel) -{ - int steps_te, steps_tv, steps_tb = 0; - double tv, tb, te = 0.0; - - // Calculate the Ramp Profile Parameters - if (vel > sqrt(std::fabs(Se_max) * accl / 2)) - { - vel = sqrt(std::fabs(Se_max) * accl / 2); - } - tb = 2 * vel / accl; - te = (std::fabs(Se_max) / vel) + tb; - tv = te - tb; - - // Interpolationsteps for every timesequence - steps_tb = (double)tb / params_.profile.t_ipo; - steps_tv = (double)(tv-tb) / params_.profile.t_ipo; - steps_te = (double)(te-tv) / params_.profile.t_ipo; - - // Reconfigure timings wtih t_ipo_ - pt_.tb = steps_tb * params_.profile.t_ipo; - pt_.tv = (steps_tb + steps_tv) * params_.profile.t_ipo; - pt_.te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; - pt_.steps_tb = steps_tb; - pt_.steps_tv = steps_tv; - pt_.steps_te = steps_te; -} - -inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) -{ - std::vector array; - double accl = params_.profile.accl; - - if(pa.getCalcTe()) - { - ROS_INFO_STREAM("Longest Se.. Vel_max: " << params_.profile.vel << " accl_max: " << params_.profile.accl << " tb: " << pt_.tb << " tv: " << pt_.tv << " te: " << pt_.te); - - int i = 1; - // Calculate the sinoid profile path - for(; i <= pt_.steps_tb ; i++) - { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( params_.profile.accl*(0.25*pow(i*params_.profile.t_ipo,2) + pow(pt_.tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/pt_.tb * (i*params_.profile.t_ipo))-1)))); - } - // tb <= t <= tv - for(; i <= (pt_.steps_tb + pt_.steps_tv) ; i++) - { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( params_.profile.vel*(i*params_.profile.t_ipo-0.5*pt_.tb))); - } - // tv <= t <= te - for(; i <= (pt_.steps_tv + pt_.steps_tb + pt_.steps_te + 1) ; i++) - { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( 0.5 * params_.profile.accl *(pt_.te*(i*params_.profile.t_ipo + pt_.tb) - 0.5*(pow(i*params_.profile.t_ipo,2)+pow(pt_.te,2)+2*pow(pt_.tb,2)) + (pow(pt_.tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/pt_.tb) * (i*params_.profile.t_ipo-pt_.tv)))))); - } - } - else - { - if(std::fabs(pa.getSe()) > 0.001) - { - double vel_max; - double accl_max = params_.profile.accl; - double tb,te,tv; - double steps_tb, steps_te, steps_tv; - - // Calculate the Profile Timings - // Reconfigure accl_max and Velmax - - accl_max = 8 * std::fabs(pa.getSe())/pow(pt_.te, 2); - vel_max = accl_max * pt_.te / 4 - sqrt((pow(accl_max,2)*pow(pt_.te,2)/16) - std::fabs(pa.getSe()) * accl_max/2 ); - - tb = 2 * vel_max / accl_max; - tv = pt_.te - tb; - te = tv + tb; - - // Interpolationsteps for every timesequence - steps_tb = trunc(tb / params_.profile.t_ipo); - steps_tv = trunc((tv-tb) / params_.profile.t_ipo); - steps_te = trunc((te-tv) / params_.profile.t_ipo); - - if(isnan(tb)!=0 || isnan(tv) !=0 || isnan(te) != 0 || isnan(accl_max) != 0 || isnan(vel_max) != 0) //NaN - { - ROS_WARN("Error while calculating the optimal velocity profile! \n Using the given parameters which may cause a longer operation time."); - tb = 2 * params_.profile.vel / params_.profile.accl; - tv = pt_.te - tb; - te = tv + tb; - steps_tb = trunc(tb / params_.profile.t_ipo); - steps_tv = trunc((tv-tb) / params_.profile.t_ipo); - steps_te = trunc((te-tv) / params_.profile.t_ipo); - accl_max = params_.profile.accl; - vel_max = params_.profile.vel; - } - - ROS_INFO_STREAM("Vel_max: " << vel_max << " accl_max: " << accl_max << " tb: " << tb << " tv: " << tv << " te: " << te); - int i = 1; - // Calculate the sinoid profile path - for(; i <= steps_tb ; i++) - { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*(accl_max*(0.25*pow(i*params_.profile.t_ipo,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*params_.profile.t_ipo))-1)))); - } - // tb <= t <= tv - for(; i <= (steps_tb + steps_tv) ; i++) - { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( vel_max*(i*params_.profile.t_ipo-0.5*tb))); - } - // tv <= t <= te - for(; i <= (steps_tv + steps_tb + steps_te + 1) ; i++) - { - array.push_back(pa.getStartValue() + pa.getSe()/std::fabs(pa.getSe())*( 0.5 * accl_max *(te*(i*params_.profile.t_ipo + tb) - 0.5*(pow(i*params_.profile.t_ipo,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/tb) * (i*params_.profile.t_ipo-tv)))))); - } - } - else - { - ROS_INFO_STREAM("Se = 0"); - array.push_back(0); - } - } - - pa.setArray(array); - return true; -} - -inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_matrix[4], - const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, - geometry_msgs::Pose start) -{ - double accl = params_.profile.accl; - double vel = params_.profile.vel; - unsigned int max_steps; - - //Convert to RPY - double roll_start, pitch_start, yaw_start; - tf::Quaternion q; - tf::quaternionMsgToTF(start.orientation, q); - tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); - - std::vector linear_path, roll_path, pitch_path, yaw_path; - - cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); - cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); - cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); - cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); - cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); - - std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) - this->getProfileTimings(std::fabs(sortedMatrix[0].getSe()), accl, vel); // Calculate the velocity profile timings with respect to the largest Se - - // Calculate the paths - for(int i=0; igeneratePath(sortedMatrix[i]); - } - - // Resize the path vectors - for(int i = 0; i < sortedMatrix.size(); i++) - { - max_steps = std::max((unsigned int)sortedMatrix[i].getArray().size() , max_steps); - } - - // Re-adjust the Matrix to its originally form. Index 0 to 3 - std::vector temp_array; - cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); - for(int j = 0; j sortedMatrix[i+1].getIdx()) - { - temp = sortedMatrix[i]; - sortedMatrix[i] = sortedMatrix[i+1]; - sortedMatrix[i+1] = temp; - } - } - } - - for(int i = 0; i < sortedMatrix.size(); i++) - { - path_matrix[i] = sortedMatrix[i].getArray(); - } - - for(int i = 0; i < sortedMatrix.size(); i++) - { - if(path_matrix[i].size() < max_steps) - { - path_matrix[i].resize(max_steps, path_matrix[i].back()); - } - } - return true; -} -/* END TrajectoryProfileSinoid ******************************************************************************************/ From 5bbbb26781b324ba533625c5a3c09f52619e3692 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 20 Oct 2015 18:23:19 +0200 Subject: [PATCH 05/35] added new headers --- .../trajectory_profile_generator_ramp.h | 58 +++++++++++++++++++ .../trajectory_profile_generator_sinoid.h | 57 ++++++++++++++++++ 2 files changed, 115 insertions(+) create mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h create mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h new file mode 100644 index 00000000..63589151 --- /dev/null +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -0,0 +1,58 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_cartesian_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: September, 2015 + * + * \brief + * + ****************************************************************/ +#ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ +#define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ + + +#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h" + +/* BEGIN TrajectoryProfileRamp ****************************************************************************************/ +/// Class providing a HardwareInterface publishing velocities. +class TrajectoryProfileRamp: public TrajectoryProfileBase +{ + public: + TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) + : TrajectoryProfileBase(params) + { } + + ~TrajectoryProfileRamp() {} + + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel); + virtual bool generatePath(cob_cartesian_controller::PathArray &pa); + virtual bool calculateProfile(std::vector* path_matrix, + double Se, double Se_roll, double Se_pitch, double Se_yaw, + geometry_msgs::Pose start); + virtual std::vector getTrajectory(double start_value, double se, + double accl, double vel, double t_ipo, + double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); + + private: + cob_cartesian_controller::ProfileTimings pt_max_; +}; +/* END TrajectoryProfileRamp **********************************************************************************************/ + +#endif /* COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ */ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h new file mode 100644 index 00000000..65142101 --- /dev/null +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -0,0 +1,57 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_cartesian_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: September, 2015 + * + * \brief + * + ****************************************************************/ +#ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ +#define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ + +#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h" + +/* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ +/// Class providing a HardwareInterface publishing positions. +class TrajectoryProfileSinoid : public TrajectoryProfileBase +{ + public: + TrajectoryProfileSinoid(const cob_cartesian_controller::CartesianActionStruct& params) + : TrajectoryProfileBase(params) + { } + + ~TrajectoryProfileSinoid() {} + + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se_max, double accl, double vel); + virtual bool generatePath(cob_cartesian_controller::PathArray &pa); + virtual bool calculateProfile(std::vector* path_matrix, + double Se, double Se_roll, double Se_pitch, double Se_yaw, + geometry_msgs::Pose start); + virtual std::vector getTrajectory(double start_value, double se, + double accl, double vel, double t_ipo, + double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); + + private: + cob_cartesian_controller::ProfileTimings pt_max_; +}; +/* END TrajectoryProfileSinoid **********************************************************************************************/ + +#endif /* COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ */ From 005bdfb86e382ea6f48c61abc750c62d49959364 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 27 Oct 2015 09:47:34 +0100 Subject: [PATCH 06/35] forgot the cpp files --- .../trajectory_profile_generator_ramp.cpp | 235 ++++++++++++++++++ .../trajectory_profile_generator_sinoid.cpp | 233 +++++++++++++++++ 2 files changed, 468 insertions(+) create mode 100644 cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp create mode 100644 cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp new file mode 100644 index 00000000..c3a360a4 --- /dev/null +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -0,0 +1,235 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_cartesian_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: September, 2015 + * + * \brief + * + ****************************************************************/ + +#include "ros/ros.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" +/* BEGIN TrajectoryProfileRamp ********************************************************************************************/ +inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfileTimings(double te_max, double accl, double vel) +{ + cob_cartesian_controller::ProfileTimings pt; + int steps_te, steps_tv, steps_tb = 0; + double tv, tb, te = 0.0; + + + tb = vel / accl; +// te = (std::fabs(Se) / vel) + tb; + te = te_max + tb; + tv = te - tb; + + // Interpolationsteps for every timesequence + steps_tb = round(tb / params_.profile.t_ipo); + steps_tv = round((tv-tb) / params_.profile.t_ipo); + steps_te = round((te-tv) / params_.profile.t_ipo); + + // Reconfigure timings wtih t_ipo_ + pt.tb = steps_tb * params_.profile.t_ipo; + pt.tv = (steps_tb + steps_tv) * params_.profile.t_ipo; + pt.te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; + pt.steps_tb = steps_tb; + pt.steps_tv = steps_tv; + pt.steps_te = steps_te; + + return pt; +} + +inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray &pa) +{ + std::vector array; + cob_cartesian_controller::ProfileTimings pt; + double accl_max = params_.profile.accl; + double vel_max = params_.profile.vel; + + if(std::fabs(pa.Se_) > 0.001) + { + if(pa.calcTe_) + { + pt = pt_max_; + } + else + { + // Calculate the Profile Timings + vel_max = accl_max * pt_max_.te / 2 - sqrt((pow(accl_max,2) * pow(pt_max_.te,2)/4) - std::fabs(pa.Se_) * accl_max); + + pt = getProfileTimings(pt_max_.te, accl_max, vel_max); + } + array = getTrajectory(pa.start_value_, pa.Se_, accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); + + switch(pa.idx_) + { + case 0: + { + ROS_INFO_STREAM("Linear-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); + break; + } + case 1: + { + ROS_INFO_STREAM("Roll-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) << " Acceleration: " << accl_max); + break; + } + case 2: + { + ROS_INFO_STREAM("Pitch-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); + break; + } + case 3: + { + ROS_INFO_STREAM("Yaw-Path: " << pa.Se_ << std::setw(10) << std::left <<" Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); + break; + } + } + } + else + { + switch(pa.idx_) + { + case 1: + { + ROS_INFO_STREAM("Roll-Path: " << pa.Se_ << " Velocity: 0" << " Acceleration: 0"); + break; + } + case 2: + { + ROS_INFO_STREAM("Pitch-Path: " << pa.Se_ << " Velocity: 0" << " Acceleration: 0"); + break; + } + case 3: + { + ROS_INFO_STREAM("Yaw-Path: " << pa.Se_ << " Velocity: 0" << " Acceleration: 0"); + break; + } + } + + array.push_back(pa.start_value_); + } + pa.array_ = array; + return true; +} + +inline std::vector TrajectoryProfileRamp::getTrajectory(double start_value, double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) +{ + std::vector array; + unsigned int i = 1; + double direction = se/std::fabs(se); + + // Calculate the ramp profile path + // 0 <= t <= tb + for(; i <= steps_tb ; i++) + { + array.push_back(start_value + direction * (0.5*accl*pow((params_.profile.t_ipo*i),2))); + } + // tb <= t <= tv + for(; i <= (steps_tb + steps_tv); i++) + { + array.push_back(start_value + direction *(vel*(params_.profile.t_ipo*i) - 0.5*pow(vel,2)/accl)); + } + // tv <= t <= te + for(; i <= (steps_tv + steps_tb + steps_te + 1); i++) + { + array.push_back(start_value + direction * (vel * (te-tb) - 0.5 * accl * pow(te-(i*params_.profile.t_ipo),2))); + } + + return array; +} + +inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_matrix[4], + const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, + geometry_msgs::Pose start) +{ + double accl = params_.profile.accl; + double vel = params_.profile.vel; + unsigned int max_steps = 0; + + //Convert to RPY + double roll_start, pitch_start, yaw_start; + tf::Quaternion q; + tf::quaternionMsgToTF(start.orientation, q); + tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); + + std::vector linear_path, roll_path, pitch_path, yaw_path; + + cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); + cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); + cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); + cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); + cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); + + std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) + + // Calculate the Ramp Profile Parameters + if (vel > sqrt(std::fabs(Se) * accl)) + { + vel = sqrt(std::fabs(Se) * accl); + } + pt_max_ = this->getProfileTimings((sortedMatrix[0].Se_ / vel), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + + // Calculate the paths + for(int i=0; igeneratePath(sortedMatrix[i]); + } + + // Resize the path vectors + for(int i = 0; i < sortedMatrix.size(); i++) + { + max_steps = std::max((unsigned int)sortedMatrix[i].array_.size() , max_steps); + } + + // Re-adjust the Matrix to its originally form. Index 0 to 3 + std::vector temp_array; + cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); + + for(int j = 0; j < sortedMatrix.size(); j++) + { + for(int i = 0; i < sortedMatrix.size()-1; i++) + { + if(sortedMatrix[i].idx_ > sortedMatrix[i+1].idx_) + { + temp = sortedMatrix[i]; + sortedMatrix[i] = sortedMatrix[i+1]; + sortedMatrix[i+1] = temp; + } + } + } + + for(int i = 0; i < sortedMatrix.size(); i++) + { + path_matrix[i] = sortedMatrix[i].array_; + } + + for(int i = 0; i < sortedMatrix.size(); i++) + { + if(path_matrix[i].size() < max_steps) + { + path_matrix[i].resize(max_steps, path_matrix[i].back()); + } + } + return true; +} + + +/* END TrajectoryProfileRamp **********************************************************************************************/ + diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp new file mode 100644 index 00000000..ed5b2846 --- /dev/null +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -0,0 +1,233 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_cartesian_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: September, 2015 + * + * \brief + * + ****************************************************************/ +#include "ros/ros.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h" + +/* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ +inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProfileTimings(double te_max, double accl, double vel) +{ + cob_cartesian_controller::ProfileTimings pt; + int steps_te, steps_tv, steps_tb = 0; + double tv, tb, te = 0.0; + + tb = 2 * vel / accl; + te = te_max + tb; + tv = te - tb; + + // Interpolationsteps for every timesequence + steps_tb = round(tb / params_.profile.t_ipo); + steps_tv = round((tv-tb) / params_.profile.t_ipo); + steps_te = round((te-tv) / params_.profile.t_ipo); + + // Reconfigure timings wtih t_ipo_ + pt.tb = steps_tb * params_.profile.t_ipo; + pt.tv = (steps_tb + steps_tv) * params_.profile.t_ipo; + pt.te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; + pt.steps_tb = steps_tb; + pt.steps_tv = steps_tv; + pt.steps_te = steps_te; + + return pt; +} + +inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) +{ + std::vector array; + cob_cartesian_controller::ProfileTimings pt; + double accl_max = params_.profile.accl; + double vel_max = params_.profile.vel; + + if(std::fabs(pa.Se_) > 0.001) + { + if(pa.calcTe_) + { + pt = pt_max_; + } + else + { + // Calculate the Profile Timings + vel_max = accl_max * pt_max_.te / 4 - sqrt((pow(accl_max,2)*pow(pt_max_.te,2)/16) - std::fabs(pa.Se_) * accl_max/2 ); + + pt = getProfileTimings(pt_max_.te, accl_max, vel_max); + } + + array = getTrajectory(pa.start_value_ , pa.Se_, accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); + + switch(pa.idx_) + { + case 0: + { + ROS_INFO_STREAM("Linear-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); + break; + } + case 1: + { + ROS_INFO_STREAM("Roll-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) << " Acceleration: " << accl_max); + break; + } + case 2: + { + ROS_INFO_STREAM("Pitch-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); + break; + } + case 3: + { + ROS_INFO_STREAM("Yaw-Path: " << pa.Se_ << std::setw(10) << std::left <<" Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); + break; + } + } + } + else + { + switch(pa.idx_) + { + case 0: + { + ROS_INFO_STREAM("Linear-Path: " << pa.Se_ << " Velocity: 0" << "Acceleration: 0"); + break; + } + case 1: + { + ROS_INFO_STREAM("Roll-Path: " << pa.Se_ << " Velocity: 0" << "Acceleration: 0"); + break; + } + case 2: + { + ROS_INFO_STREAM("Pitch-Path: " << pa.Se_ << " Velocity: 0" << "Acceleration: 0"); + break; + } + case 3: + { + ROS_INFO_STREAM("Yaw-Path: " << pa.Se_ << " Velocity: 0" << "Acceleration: 0"); + break; + } + } + array.push_back(pa.start_value_); + } + + pa.array_ = array; + return true; +} + +inline std::vector TrajectoryProfileSinoid::getTrajectory(double start_value, double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) +{ + std::vector array; + unsigned int i = 1; + double direction = se/std::fabs(se); + //Calculate the sinoid profile path + for(; i <= steps_tb ; i++) + { + array.push_back(start_value + direction * (accl*(0.25*pow(i*t_ipo,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*t_ipo))-1)))); + } + // tb <= t <= tv + for(; i <= (steps_tb + steps_tv) ; i++) + { + array.push_back(start_value + direction * ( vel*(i*t_ipo-0.5*tb))); + } + // tv <= t <= te + for(; i <= (steps_tv + steps_tb + steps_te + 1) ; i++) + { + array.push_back(start_value + direction * (0.5 * accl *(te*(i*t_ipo + tb) - 0.5*(pow(i*t_ipo,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/tb) * (i*t_ipo-tv)))))); + } + return array; +} + +inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_matrix[4], + const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, + geometry_msgs::Pose start) +{ + double accl = params_.profile.accl; + double vel = params_.profile.vel; + unsigned int max_steps = 0; + + //Convert to RPY + double roll_start, pitch_start, yaw_start; + tf::Quaternion q; + tf::quaternionMsgToTF(start.orientation, q); + tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); + + std::vector linear_path, roll_path, pitch_path, yaw_path; + + cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); + cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); + cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); + cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); + cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); + + std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) + + // Calculate the Sinoid Profile Parameters + if (vel > sqrt(std::fabs(sortedMatrix[0].Se_) * accl / 2)) + { + vel = sqrt(std::fabs(sortedMatrix[0].Se_) * accl / 2); + } + pt_max_ = this->getProfileTimings((std::fabs(sortedMatrix[0].Se_) / vel), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + + // Calculate the paths + for(int i=0; igeneratePath(sortedMatrix[i]); + } + + // Resize the path vectors + for(int i = 0; i < sortedMatrix.size(); i++) + { + max_steps = std::max((unsigned int)sortedMatrix[i].array_.size() , max_steps); + } + + // Re-adjust the Matrix to its originally form. Index 0 to 3 + std::vector temp_array; + cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); + for(int j = 0; j sortedMatrix[i+1].idx_) + { + temp = sortedMatrix[i]; + sortedMatrix[i] = sortedMatrix[i+1]; + sortedMatrix[i+1] = temp; + } + } + } + + for(int i = 0; i < sortedMatrix.size(); i++) + { + path_matrix[i] = sortedMatrix[i].array_; + } + + for(int i = 0; i < sortedMatrix.size(); i++) + { + if(path_matrix[i].size() < max_steps) + { + path_matrix[i].resize(max_steps, path_matrix[i].back()); + } + } + return true; +} +/* END TrajectoryProfileSinoid ******************************************************************************************/ + From c9b1ac30e4419a3fa57657f2994ce49366eccc0a Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 27 Oct 2015 15:09:10 +0100 Subject: [PATCH 07/35] code reduction part 2. --- .../cartesian_controller_utils.h | 5 + .../trajactory_profile_generator_base.h | 6 +- .../trajectory_profile_generator_ramp.h | 4 +- .../trajectory_profile_generator_sinoid.h | 4 +- .../src/cartesian_controller.cpp | 2 +- .../src/cartesian_controller_utils.cpp | 44 +++++++++ .../simple_cartesian_interface.py | 4 +- .../trajectory_profile_generator_ramp.cpp | 97 ++++++------------- .../trajectory_profile_generator_sinoid.cpp | 93 +++++------------- 9 files changed, 111 insertions(+), 148 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h index 6a61a6fd..6d836bf0 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h @@ -35,6 +35,8 @@ #include #include +#include "cob_cartesian_controller/cartesian_controller_data_types.h" + class CartesianControllerUtils @@ -54,6 +56,9 @@ class CartesianControllerUtils void previewPath(const geometry_msgs::PoseArray& pose_array); + void sortMatrixByIdx(std::vector &m); + void adjustArrayLength(std::vector &m); + void copyMatrix(std::vector *path_array,std::vector &m); private: ros::NodeHandle nh_; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h index bdff75b5..203bb8ca 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h @@ -43,21 +43,21 @@ class TrajectoryProfileBase virtual ~TrajectoryProfileBase() {} - + virtual cob_cartesian_controller::ProfileTimings getMaxProfileTimings(double Se_max, double accl, double vel) = 0; virtual bool calculateProfile(std::vector path_matrix[4], double Se, double Se_roll, double Se_pitch, double Se_yaw, geometry_msgs::Pose start) = 0; - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel) = 0; virtual std::vector getTrajectory(double start_value, double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; private: virtual bool generatePath(cob_cartesian_controller::PathArray &pa) = 0; + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel) = 0; + protected: const cob_cartesian_controller::CartesianActionStruct ¶ms_; double vel_max_; }; - #endif /* HARDWARE_INTERFACE_TYPE_BASE_H_ */ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index 63589151..fdcf47aa 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -40,8 +40,7 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase { } ~TrajectoryProfileRamp() {} - - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel); + virtual cob_cartesian_controller::ProfileTimings getMaxProfileTimings(double Se_max, double accl, double vel); virtual bool generatePath(cob_cartesian_controller::PathArray &pa); virtual bool calculateProfile(std::vector* path_matrix, double Se, double Se_roll, double Se_pitch, double Se_yaw, @@ -51,6 +50,7 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); private: + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel); cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileRamp **********************************************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 65142101..0bfc1733 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -39,8 +39,7 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase { } ~TrajectoryProfileSinoid() {} - - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se_max, double accl, double vel); + virtual cob_cartesian_controller::ProfileTimings getMaxProfileTimings(double Se_max, double accl, double vel); virtual bool generatePath(cob_cartesian_controller::PathArray &pa); virtual bool calculateProfile(std::vector* path_matrix, double Se, double Se_roll, double Se_pitch, double Se_yaw, @@ -50,6 +49,7 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); private: + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se_max, double accl, double vel); cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileSinoid **********************************************************************************************/ diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index 711d7f87..10fb8247 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -173,7 +173,7 @@ bool CartesianController::movePTP(const geometry_msgs::Pose& target_pose, const tf_broadcaster_.sendTransform(stamped_transform); // Get transformation - stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); + tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); // Wait for chain_tip_link to be within epsilon area of target_frame if(utils_.inEpsilonArea(stamped_transform, epsilon)) diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index 6582bbad..8543fdc5 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -158,3 +158,47 @@ void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray& pose_ marker_pub_.publish(marker_array); } + +void CartesianControllerUtils::sortMatrixByIdx(std::vector &m) +{ + std::vector temp_array; + cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); + + for(int j = 0; j < m.size(); j++) + { + for(int i = 0; i < m.size()-1; i++) + { + if(m[i].idx_ > m[i+1].idx_) + { + temp = m[i]; + m[i] = m[i+1]; + m[i+1] = temp; + } + } + } +} + +void CartesianControllerUtils::adjustArrayLength(std::vector &m) +{ + unsigned int max_steps = 0; + for(int i = 0; i < m.size(); i++) + { + max_steps = std::max((unsigned int)m[i].array_.size() , max_steps); + } + + for(int i = 0; i < m.size(); i++) + { + if(m[i].array_.size() < max_steps) + { + m[i].array_.resize(max_steps, m[i].array_.back()); + } + } +} + +void CartesianControllerUtils::copyMatrix(std::vector *path_array,std::vector &m) +{ + for(int i = 0; i < m.size(); i++) + { + path_array[i] = m[i].array_; + } +} diff --git a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py index b7cf9488..528986a1 100644 --- a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py +++ b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py @@ -21,7 +21,7 @@ def move_lin(pose_goal, frame_id, profile, rotate_only=False): goal.move_lin.pose_goal = pose_goal goal.move_lin.frame_id = frame_id goal.move_lin.rotate_only = rotate_only - goal.move_lin.profile = profile + goal.profile = profile # print goal client.send_goal(goal) @@ -48,7 +48,7 @@ def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile, ro goal.move_circ.start_angle = start_angle goal.move_circ.end_angle = end_angle goal.move_circ.radius = radius - goal.move_circ.profile = profile + goal.profile = profile print goal client.send_goal(goal) diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index c3a360a4..558c91c6 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -34,7 +34,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil int steps_te, steps_tv, steps_tb = 0; double tv, tb, te = 0.0; - tb = vel / accl; // te = (std::fabs(Se) / vel) + tb; te = te_max + tb; @@ -52,10 +51,20 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil pt.steps_tb = steps_tb; pt.steps_tv = steps_tv; pt.steps_te = steps_te; - return pt; } +inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getMaxProfileTimings(double Se_max, double accl, double vel) +{ + // Calculate the Ramp Profile Parameters + if (vel > sqrt(std::fabs(Se_max) * accl)) + { + vel = sqrt(std::fabs(Se_max) * accl); + } + + return getProfileTimings((std::fabs(Se_max) / vel), accl, vel); +} + inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray &pa) { std::vector array; @@ -73,9 +82,9 @@ inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathAr { // Calculate the Profile Timings vel_max = accl_max * pt_max_.te / 2 - sqrt((pow(accl_max,2) * pow(pt_max_.te,2)/4) - std::fabs(pa.Se_) * accl_max); - pt = getProfileTimings(pt_max_.te, accl_max, vel_max); } + array = getTrajectory(pa.start_value_, pa.Se_, accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); switch(pa.idx_) @@ -104,25 +113,6 @@ inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathAr } else { - switch(pa.idx_) - { - case 1: - { - ROS_INFO_STREAM("Roll-Path: " << pa.Se_ << " Velocity: 0" << " Acceleration: 0"); - break; - } - case 2: - { - ROS_INFO_STREAM("Pitch-Path: " << pa.Se_ << " Velocity: 0" << " Acceleration: 0"); - break; - } - case 3: - { - ROS_INFO_STREAM("Yaw-Path: " << pa.Se_ << " Velocity: 0" << " Acceleration: 0"); - break; - } - } - array.push_back(pa.start_value_); } pa.array_ = array; @@ -159,77 +149,44 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, geometry_msgs::Pose start) { - double accl = params_.profile.accl; - double vel = params_.profile.vel; - unsigned int max_steps = 0; + CartesianControllerUtils ccu; + std::vector linear_path, roll_path, pitch_path, yaw_path; + std::vector sortedMatrix; + double roll_start, pitch_start, yaw_start; //Convert to RPY - double roll_start, pitch_start, yaw_start; tf::Quaternion q; tf::quaternionMsgToTF(start.orientation, q); tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); - std::vector linear_path, roll_path, pitch_path, yaw_path; - cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); + cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); - std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) + // Sort the Matrix from the largest Se (0) to the smallest one (3) + sortedMatrix = pm.getSortedMatrix(); - // Calculate the Ramp Profile Parameters - if (vel > sqrt(std::fabs(Se) * accl)) - { - vel = sqrt(std::fabs(Se) * accl); - } - pt_max_ = this->getProfileTimings((sortedMatrix[0].Se_ / vel), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + // Get the profile timings from the longest path + pt_max_ = getMaxProfileTimings(sortedMatrix[0].Se_, params_.profile.accl, params_.profile.vel); // Calculate the paths - for(int i=0; igeneratePath(sortedMatrix[i]); - } - - // Resize the path vectors for(int i = 0; i < sortedMatrix.size(); i++) { - max_steps = std::max((unsigned int)sortedMatrix[i].array_.size() , max_steps); + generatePath(sortedMatrix[i]); } - // Re-adjust the Matrix to its originally form. Index 0 to 3 - std::vector temp_array; - cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); + // Adjust the array length + ccu.adjustArrayLength(sortedMatrix); - for(int j = 0; j < sortedMatrix.size(); j++) - { - for(int i = 0; i < sortedMatrix.size()-1; i++) - { - if(sortedMatrix[i].idx_ > sortedMatrix[i+1].idx_) - { - temp = sortedMatrix[i]; - sortedMatrix[i] = sortedMatrix[i+1]; - sortedMatrix[i+1] = temp; - } - } - } + // Sort the arrays in the following order lin(0), roll(1), pitch(2), yaw(3) + ccu.sortMatrixByIdx(sortedMatrix); - for(int i = 0; i < sortedMatrix.size(); i++) - { - path_matrix[i] = sortedMatrix[i].array_; - } + ccu.copyMatrix(path_matrix,sortedMatrix); - for(int i = 0; i < sortedMatrix.size(); i++) - { - if(path_matrix[i].size() < max_steps) - { - path_matrix[i].resize(max_steps, path_matrix[i].back()); - } - } return true; } - /* END TrajectoryProfileRamp **********************************************************************************************/ - diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index ed5b2846..b4589555 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -54,6 +54,17 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf return pt; } +inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getMaxProfileTimings(double Se_max, double accl, double vel) +{ + // Calculate the Sinoid Profile Parameters + if (vel > sqrt(std::fabs(Se_max) * accl / 2)) + { + vel = sqrt(std::fabs(Se_max) * accl / 2); + } + return getProfileTimings((std::fabs(Se_max) / vel), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + +} + inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) { std::vector array; @@ -74,7 +85,6 @@ inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::Path pt = getProfileTimings(pt_max_.te, accl_max, vel_max); } - array = getTrajectory(pa.start_value_ , pa.Se_, accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); switch(pa.idx_) @@ -103,29 +113,6 @@ inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::Path } else { - switch(pa.idx_) - { - case 0: - { - ROS_INFO_STREAM("Linear-Path: " << pa.Se_ << " Velocity: 0" << "Acceleration: 0"); - break; - } - case 1: - { - ROS_INFO_STREAM("Roll-Path: " << pa.Se_ << " Velocity: 0" << "Acceleration: 0"); - break; - } - case 2: - { - ROS_INFO_STREAM("Pitch-Path: " << pa.Se_ << " Velocity: 0" << "Acceleration: 0"); - break; - } - case 3: - { - ROS_INFO_STREAM("Yaw-Path: " << pa.Se_ << " Velocity: 0" << "Acceleration: 0"); - break; - } - } array.push_back(pa.start_value_); } @@ -160,32 +147,28 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, geometry_msgs::Pose start) { - double accl = params_.profile.accl; - double vel = params_.profile.vel; - unsigned int max_steps = 0; + CartesianControllerUtils ccu; + std::vector linear_path, roll_path, pitch_path, yaw_path; + std::vector sortedMatrix; + double roll_start, pitch_start, yaw_start; //Convert to RPY - double roll_start, pitch_start, yaw_start; tf::Quaternion q; tf::quaternionMsgToTF(start.orientation, q); tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); - std::vector linear_path, roll_path, pitch_path, yaw_path; - cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); + cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); - std::vector sortedMatrix = pm.getSortedMatrix(); // Sort the Matrix from the largest Se (0) to the smallest one (3) + // Sort the Matrix from the largest Se (0) to the smallest one (3) + sortedMatrix = pm.getSortedMatrix(); - // Calculate the Sinoid Profile Parameters - if (vel > sqrt(std::fabs(sortedMatrix[0].Se_) * accl / 2)) - { - vel = sqrt(std::fabs(sortedMatrix[0].Se_) * accl / 2); - } - pt_max_ = this->getProfileTimings((std::fabs(sortedMatrix[0].Se_) / vel), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + // Get the profile timings from the longest path + pt_max_ = getMaxProfileTimings(sortedMatrix[0].Se_, params_.profile.accl, params_.profile.vel); // Calculate the paths for(int i=0; i path_m this->generatePath(sortedMatrix[i]); } - // Resize the path vectors - for(int i = 0; i < sortedMatrix.size(); i++) - { - max_steps = std::max((unsigned int)sortedMatrix[i].array_.size() , max_steps); - } + // Adjust the array length + ccu.adjustArrayLength(sortedMatrix); - // Re-adjust the Matrix to its originally form. Index 0 to 3 - std::vector temp_array; - cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); - for(int j = 0; j sortedMatrix[i+1].idx_) - { - temp = sortedMatrix[i]; - sortedMatrix[i] = sortedMatrix[i+1]; - sortedMatrix[i+1] = temp; - } - } - } + // Sort the arrays in the following order lin(0), roll(1), pitch(2), yaw(3) + ccu.sortMatrixByIdx(sortedMatrix); - for(int i = 0; i < sortedMatrix.size(); i++) - { - path_matrix[i] = sortedMatrix[i].array_; - } + ccu.copyMatrix(path_matrix,sortedMatrix); - for(int i = 0; i < sortedMatrix.size(); i++) - { - if(path_matrix[i].size() < max_steps) - { - path_matrix[i].resize(max_steps, path_matrix[i].back()); - } - } return true; } /* END TrajectoryProfileSinoid ******************************************************************************************/ From e8b0bdf1f317e6febb4baf0ea00f3af0f481370e Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 17 Nov 2015 18:01:26 +0100 Subject: [PATCH 08/35] Bug fixes + code reduction --- cob_cartesian_controller/CMakeLists.txt | 2 +- .../cartesian_controller_data_types.h | 45 ++---- .../cartesian_controller_utils.h | 2 + .../trajectory_interpolator.h | 2 +- .../trajectory_profile_generator_base.h | 63 ++++++++ .../trajectory_profile_generator_builder.h | 45 ++++++ .../trajectory_profile_generator_ramp.h | 6 +- .../trajectory_profile_generator_sinoid.h | 6 +- .../src/cartesian_controller.cpp | 14 ++ .../src/cartesian_controller_utils.cpp | 40 ++---- .../trajectory_interpolator.cpp | 53 ++++--- .../trajectory_profile_generator_builder.cpp | 55 +++++++ .../trajectory_profile_generator_ramp.cpp | 133 +++++++---------- .../trajectory_profile_generator_sinoid.cpp | 135 ++++++++---------- 14 files changed, 356 insertions(+), 245 deletions(-) create mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h create mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h create mode 100644 cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp diff --git a/cob_cartesian_controller/CMakeLists.txt b/cob_cartesian_controller/CMakeLists.txt index 82a5f455..43ea7ac5 100644 --- a/cob_cartesian_controller/CMakeLists.txt +++ b/cob_cartesian_controller/CMakeLists.txt @@ -38,7 +38,7 @@ add_library(cartesian_controller_utils src/cartesian_controller_utils.cpp) add_dependencies(cartesian_controller_utils ${catkin_EXPORTED_TARGETS}) target_link_libraries(cartesian_controller_utils ${catkin_LIBRARIES}) -add_library(profile_generator src/trajectory_profile_generator/trajectory_profile_generator.cpp src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp) +add_library(profile_generator src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp) add_dependencies(profile_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(profile_generator ${catkin_LIBRARIES}) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 55e1bbd3..e1fcd13e 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -47,6 +47,7 @@ namespace cob_cartesian_controller { double tb, te, tv; unsigned int steps_tb, steps_te, steps_tv; + bool ok; }; struct MoveLinStruct @@ -115,52 +116,22 @@ namespace cob_cartesian_controller pm_.clear(); } - std::vector getSortedMatrix(); - - private: - void sortMatrixRows(); + double getMaxSe(); std::vector pm_; - }; - inline std::vector PathMatrix::getSortedMatrix() + inline double PathMatrix::getMaxSe() { - sortMatrixRows(); - return pm_; - } + double se_max = 0; - inline void PathMatrix::sortMatrixRows() - { - std::vector tmp; - PathArray temp(0, 0, 0.0, tmp); - for(int j = 0; j pm_[i+1].idx_) -// { -// temp = pm_[i]; -// pm_[i] = pm_[i+1]; -// pm_[i+1] = temp; -// } -// } -// } - - pm_[0].calcTe_ = true; // Reference Time for profile interpolation (Te) + return se_max; } - - }//namespace #endif /* COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_ */ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h index 6d836bf0..0b9863f1 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h @@ -59,10 +59,12 @@ class CartesianControllerUtils void sortMatrixByIdx(std::vector &m); void adjustArrayLength(std::vector &m); void copyMatrix(std::vector *path_array,std::vector &m); + double roundUpToMultiplier(double numberToRound, double multiplier); private: ros::NodeHandle nh_; tf::TransformListener tf_listener_; + visualization_msgs::MarkerArray marker_array_; ros::Publisher marker_pub_; }; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h index 729198c5..54affb65 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h @@ -34,7 +34,7 @@ #include #include -#include +#include diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h new file mode 100644 index 00000000..007039ea --- /dev/null +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -0,0 +1,63 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_twist_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: June, 2015 + * + * \brief + * This header contains the interface description of all available + * hardware interface types (position/velocity). + * + ****************************************************************/ +#ifndef HARDWARE_INTERFACE_TYPE_BASE_H_ +#define HARDWARE_INTERFACE_TYPE_BASE_H_ + +#include "ros/ros.h" +#include "cob_cartesian_controller/cartesian_controller_data_types.h" +#include + +/// Base class for hardware interfaces types. +class TrajectoryProfileBase +{ + public: + TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct& params): + params_(params) + {vel_max_=0;} + + virtual ~TrajectoryProfileBase() {} + + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel) = 0; + virtual bool calculateProfile(std::vector path_matrix[4], + double Se, double Se_roll, double Se_pitch, double Se_yaw, + geometry_msgs::Pose start) = 0; + virtual std::vector getTrajectory(double start_value, double se, + double accl, double vel, double t_ipo, + double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; + private: + virtual bool generatePath(cob_cartesian_controller::PathArray &pa) = 0; +// virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel) = 0; + + + protected: + const cob_cartesian_controller::CartesianActionStruct ¶ms_; + double vel_max_; +}; + +#endif /* HARDWARE_INTERFACE_TYPE_BASE_H_ */ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h new file mode 100644 index 00000000..9f7b8d33 --- /dev/null +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h @@ -0,0 +1,45 @@ +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_cartesian_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: September, 2015 + * + * \brief + * + ****************************************************************/ +#ifndef TRAJECTORY_PROFILE_BUILDER_H_ +#define TRAJECTORY_PROFILE_BUILDER_H_ + +#include "cob_cartesian_controller/cartesian_controller_data_types.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h" + + +/* BEGIN TrajectoryProfileBuilder *****************************************************************************************/ +class TrajectoryProfileBuilder +{ + public: + TrajectoryProfileBuilder() {} + ~TrajectoryProfileBuilder() {} + + static TrajectoryProfileBase* createProfile(const cob_cartesian_controller::CartesianActionStruct& params); +}; +/* END TrajectoryGeneratorBuilder *******************************************************************************************/ + +#endif diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index fdcf47aa..05a183a5 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -28,7 +28,7 @@ #define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ -#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h" /* BEGIN TrajectoryProfileRamp ****************************************************************************************/ /// Class providing a HardwareInterface publishing velocities. @@ -40,7 +40,7 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase { } ~TrajectoryProfileRamp() {} - virtual cob_cartesian_controller::ProfileTimings getMaxProfileTimings(double Se_max, double accl, double vel); + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel); virtual bool generatePath(cob_cartesian_controller::PathArray &pa); virtual bool calculateProfile(std::vector* path_matrix, double Se, double Se_roll, double Se_pitch, double Se_yaw, @@ -50,7 +50,7 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); private: - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel); +// virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel); cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileRamp **********************************************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 0bfc1733..b26f2422 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -27,7 +27,7 @@ #ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ #define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ -#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h" /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ /// Class providing a HardwareInterface publishing positions. @@ -39,7 +39,7 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase { } ~TrajectoryProfileSinoid() {} - virtual cob_cartesian_controller::ProfileTimings getMaxProfileTimings(double Se_max, double accl, double vel); + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel); virtual bool generatePath(cob_cartesian_controller::PathArray &pa); virtual bool calculateProfile(std::vector* path_matrix, double Se, double Se_roll, double Se_pitch, double Se_yaw, @@ -49,7 +49,7 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); private: - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se_max, double accl, double vel); +// virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel); cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileSinoid **********************************************************************************************/ diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index 10fb8247..9a8c5fe3 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -254,6 +254,7 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca void CartesianController::goalCallback() { + geometry_msgs::Pose start_pose, end_pose, pose; geometry_msgs::PoseArray cartesian_path; cob_cartesian_controller::CartesianActionStruct action_struct; @@ -262,6 +263,9 @@ void CartesianController::goalCallback() ROS_INFO("Received a new goal"); ROS_INFO_STREAM("========================================================================="); + start_pose = utils_.getPose(root_frame_, chain_tip_link_); + ROS_INFO_STREAM("Start Pose\n X: " << start_pose.position.x << ", Y:" << start_pose.position.y << " , Z: " << start_pose.position.z); + action_struct = acceptGoal(as_->acceptNewGoal()); if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::LIN) @@ -276,6 +280,8 @@ void CartesianController::goalCallback() } // Publish Preview +// visualization_msgs::MarkerArray marker_array; +// utils_.previewPath(cartesian_path,marker_array); utils_.previewPath(cartesian_path); // Activate Tracking @@ -292,6 +298,14 @@ void CartesianController::goalCallback() return; } +// movePTP(action_struct.move_lin.end, 0.005); + +// end_pose = utils_.getPose(root_frame_,chain_tip_link_); +// +// ROS_INFO_STREAM("Relative \n X: " << fabs(end_pose.position.x) - fabs(action_struct.move_lin.end.position.x) << +// ", Y:" << fabs(end_pose.position.y) - fabs(action_struct.move_lin.end.position.y) << +// " , Z: " << fabs(end_pose.position.z) - fabs(action_struct.move_lin.end.position.z)); + // De-Activate Tracking if(!stopTracking()) { diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index 8543fdc5..0ec3eeed 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -133,10 +133,8 @@ void CartesianControllerUtils::poseToRPY(const geometry_msgs::Pose& pose, double void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray& pose_array) { - visualization_msgs::MarkerArray marker_array; - visualization_msgs::Marker marker; - marker.type = visualization_msgs::Marker::ARROW; + marker.type = visualization_msgs::Marker::SPHERE; marker.lifetime = ros::Duration(); marker.action = visualization_msgs::Marker::ADD; marker.header = pose_array.header; @@ -149,33 +147,16 @@ void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray& pose_ marker.color.b = 1.0; marker.color.a = 1.0; - for(unsigned int i=0; i &m) -{ - std::vector temp_array; - cob_cartesian_controller::PathArray temp(0, 0, 0.0, temp_array); + double id = marker_array_.markers.size(); - for(int j = 0; j < m.size(); j++) - { - for(int i = 0; i < m.size()-1; i++) + for(unsigned int i=0; i < pose_array.poses.size(); i++) { - if(m[i].idx_ > m[i+1].idx_) - { - temp = m[i]; - m[i] = m[i+1]; - m[i+1] = temp; - } + marker.id = id+i; + marker.pose = pose_array.poses.at(i); + marker_array_.markers.push_back(marker); } - } + + marker_pub_.publish(marker_array_); } void CartesianControllerUtils::adjustArrayLength(std::vector &m) @@ -202,3 +183,8 @@ void CartesianControllerUtils::copyMatrix(std::vector *path_array,std::v path_array[i] = m[i].array_; } } + +double CartesianControllerUtils::roundUpToMultiplier(double numberToRound, double multiplier) +{ + return ( multiplier - fmod(numberToRound,multiplier) + numberToRound ); +} diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index e6acd9b8..ab085046 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -27,18 +27,19 @@ ****************************************************************/ #include -#include +#include +#include bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_array, const cob_cartesian_controller::CartesianActionStruct& as) { - + double Se_roll, Se_pitch, Se_yaw; this->trajectory_profile_generator_.reset(TrajectoryProfileBuilder::createProfile(as)); pose_array.header.stamp = ros::Time::now(); pose_array.header.frame_id = root_frame_; - tf::Quaternion q; + tf::Quaternion q_start, q_end, q_relative, q; double start_roll, start_pitch, start_yaw; double end_roll, end_pitch, end_yaw; @@ -50,18 +51,26 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ pow((as.move_lin.end.position.y - as.move_lin.start.position.y), 2) + pow((as.move_lin.end.position.z - as.move_lin.start.position.z), 2)); - // Convert Quaternions into RPY Angles for start and end pose - tf::quaternionMsgToTF(as.move_lin.start.orientation, q); - tf::Matrix3x3(q).getRPY(start_roll, start_pitch, start_yaw); - - tf::quaternionMsgToTF(as.move_lin.end.orientation, q); - tf::Matrix3x3(q).getRPY(end_roll, end_pitch, end_yaw); +// // Convert Quaternions into RPY Angles for start and end pose + tf::quaternionMsgToTF(as.move_lin.start.orientation, q_start); +//// tf::Matrix3x3(q).getRPY(start_roll, start_pitch, start_yaw); +// + tf::quaternionMsgToTF(as.move_lin.end.orientation, q_end); +//// tf::Matrix3x3(q).getRPY(end_roll, end_pitch, end_yaw); +// q_relative = q_start * q_end.inverse(); +// q_relative.normalized(); +// +// q_relative = q_start * q_end.inverse(); +// tf::Matrix3x3(q_relative).getRPY(Se_roll, Se_pitch, Se_yaw); // Calculate path length for the angular movement - double Se_roll, Se_pitch, Se_yaw; - Se_roll = end_roll - start_roll; - Se_pitch = end_pitch - start_pitch; - Se_yaw = end_yaw - start_yaw; + Se_roll = std::fabs(end_roll) - std::fabs(start_roll); + Se_pitch = std::fabs(end_pitch) - std::fabs(start_pitch); + Se_yaw = std::fabs(end_yaw) - std::fabs(start_yaw); + +// ROS_INFO_STREAM("Q_x: " << Se_roll << +// " Q_y: " << Se_pitch << +// " Q_z: " << Se_yaw); // Calculate path for each Angle if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, Se_roll, Se_pitch, Se_yaw, as.move_lin.start)) @@ -75,11 +84,21 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ yaw_path = path_matrix[3]; // Interpolate the linear path - for(int i = 0 ; i < path_matrix[0].size() ; i++) + for(unsigned int i = 0 ; i < path_matrix[0].size() ; i++) { - pose.position.x = as.move_lin.start.position.x + linear_path.at(i) * (as.move_lin.end.position.x - as.move_lin.start.position.x) / Se; - pose.position.y = as.move_lin.start.position.y + linear_path.at(i) * (as.move_lin.end.position.y - as.move_lin.start.position.y) / Se; - pose.position.z = as.move_lin.start.position.z + linear_path.at(i) * (as.move_lin.end.position.z - as.move_lin.start.position.z) / Se; + if(linear_path.back() == 0) + { + pose.position.x = as.move_lin.start.position.x; + pose.position.y = as.move_lin.start.position.y; + pose.position.z = as.move_lin.start.position.z; + } + else + { + ROS_INFO_STREAM("linear.path.at(..) = " << linear_path.at(i)); + pose.position.x = as.move_lin.start.position.x + linear_path.at(i) * (as.move_lin.end.position.x - as.move_lin.start.position.x) / linear_path.back(); + pose.position.y = as.move_lin.start.position.y + linear_path.at(i) * (as.move_lin.end.position.y - as.move_lin.start.position.y) / linear_path.back(); + pose.position.z = as.move_lin.start.position.z + linear_path.at(i) * (as.move_lin.end.position.z - as.move_lin.start.position.z) / linear_path.back(); + } // Transform RPY to Quaternion q.setRPY(roll_path.at(i), pitch_path.at(i), yaw_path.at(i)); diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp new file mode 100644 index 00000000..ccd5fdc7 --- /dev/null +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp @@ -0,0 +1,55 @@ + +/*! + ***************************************************************** + * \file + * + * \note + * Copyright (c) 2015 \n + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) \n\n + * + ***************************************************************** + * + * \note + * Project name: care-o-bot + * \note + * ROS stack name: cob_control + * \note + * ROS package name: cob_twist_controller + * + * \author + * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com + * + * \date Date of creation: June, 2015 + * + * \brief + * This module contains the implementation of all interface types. + * + ****************************************************************/ +#define RAMP_PROFILE 1 +#define SINOID_PROFILE 2 +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" +#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h" + +/* BEGIN TrajectoryProfileBase *****************************************************************************************/ + +TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesian_controller::CartesianActionStruct& params) +{ + TrajectoryProfileBase* ib = NULL; + switch(params.profile.profile_type) + { + case RAMP_PROFILE: + ib = new TrajectoryProfileRamp(params); + break; + case SINOID_PROFILE: + ib = new TrajectoryProfileSinoid(params); + break; + default: + ROS_ERROR("Unknown Profile"); + break; + } + + return ib; +} +/* END TrajectoryProfileBase *******************************************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index 558c91c6..e90a0ab4 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -28,43 +28,57 @@ #include "ros/ros.h" #include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" /* BEGIN TrajectoryProfileRamp ********************************************************************************************/ -inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfileTimings(double te_max, double accl, double vel) +inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfileTimings(double Se, double te, double accl, double vel) { - cob_cartesian_controller::ProfileTimings pt; - int steps_te, steps_tv, steps_tb = 0; - double tv, tb, te = 0.0; + CartesianControllerUtils utils; + cob_cartesian_controller::ProfileTimings pt; + int steps_te, steps_tv, steps_tb = 0; + double tv, tb = 0.0; + + // Calculate the Sinoid Profile Parameters + if (vel > sqrt(std::fabs(Se) * accl)) + { + vel = sqrt(std::fabs(Se) * accl); + } - tb = vel / accl; -// te = (std::fabs(Se) / vel) + tb; - te = te_max + tb; + //Todo: Find a value. + if(vel > 0.001) + { + tb = utils.roundUpToMultiplier(vel / accl, params_.profile.t_ipo); + if(te == 0) + { + te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb, params_.profile.t_ipo); + } tv = te - tb; + ROS_INFO_STREAM("========================================================================="); + ROS_INFO_STREAM("vel_new:" << vel << " accl: " << accl << " Se: " << std::fabs(Se)); + ROS_INFO_STREAM("tb:" << tb << " tv: " << tv << " te: " << te); + // Interpolationsteps for every timesequence - steps_tb = round(tb / params_.profile.t_ipo); - steps_tv = round((tv-tb) / params_.profile.t_ipo); - steps_te = round((te-tv) / params_.profile.t_ipo); - - // Reconfigure timings wtih t_ipo_ - pt.tb = steps_tb * params_.profile.t_ipo; - pt.tv = (steps_tb + steps_tv) * params_.profile.t_ipo; - pt.te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; - pt.steps_tb = steps_tb; - pt.steps_tv = steps_tv; - pt.steps_te = steps_te; - return pt; -} + pt.tb = tb; + pt.tv = tv; + pt.te = te; -inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getMaxProfileTimings(double Se_max, double accl, double vel) -{ - // Calculate the Ramp Profile Parameters - if (vel > sqrt(std::fabs(Se_max) * accl)) + pt.steps_tb = pt.tb/params_.profile.t_ipo; + pt.steps_tv = (pt.tv-pt.tb)/params_.profile.t_ipo; + pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo; + + ROS_INFO_STREAM("pt.tb:" << pt.tb << " pt.tv: " << pt.tv << " pt.te: " << pt.te); + ROS_INFO_STREAM("pt.steps_tb:" << pt.steps_tb << " pt.steps_tv: " << pt.steps_tv << " pt.steps_te: " << pt.steps_te); + ROS_INFO_STREAM("========================================================================="); + + pt.ok = true; + } + else { - vel = sqrt(std::fabs(Se_max) * accl); + pt.ok = false; } - return getProfileTimings((std::fabs(Se_max) / vel), accl, vel); + return pt; } + inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray &pa) { std::vector array; @@ -72,49 +86,17 @@ inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathAr double accl_max = params_.profile.accl; double vel_max = params_.profile.vel; - if(std::fabs(pa.Se_) > 0.001) + // Calculate the Profile Timings + pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max); + if(pt.ok) { - if(pa.calcTe_) - { - pt = pt_max_; - } - else - { - // Calculate the Profile Timings - vel_max = accl_max * pt_max_.te / 2 - sqrt((pow(accl_max,2) * pow(pt_max_.te,2)/4) - std::fabs(pa.Se_) * accl_max); - pt = getProfileTimings(pt_max_.te, accl_max, vel_max); - } - - array = getTrajectory(pa.start_value_, pa.Se_, accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); - - switch(pa.idx_) - { - case 0: - { - ROS_INFO_STREAM("Linear-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); - break; - } - case 1: - { - ROS_INFO_STREAM("Roll-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) << " Acceleration: " << accl_max); - break; - } - case 2: - { - ROS_INFO_STREAM("Pitch-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); - break; - } - case 3: - { - ROS_INFO_STREAM("Yaw-Path: " << pa.Se_ << std::setw(10) << std::left <<" Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); - break; - } - } + array = getTrajectory(pa.start_value_ , std::fabs(pa.Se_), accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } else { array.push_back(pa.start_value_); } + pa.array_ = array; return true; } @@ -129,17 +111,17 @@ inline std::vector TrajectoryProfileRamp::getTrajectory(double start_val // 0 <= t <= tb for(; i <= steps_tb ; i++) { - array.push_back(start_value + direction * (0.5*accl*pow((params_.profile.t_ipo*i),2))); + array.push_back(start_value + direction * (0.5*accl*pow((t_ipo*i),2))); } // tb <= t <= tv for(; i <= (steps_tb + steps_tv); i++) { - array.push_back(start_value + direction *(vel*(params_.profile.t_ipo*i) - 0.5*pow(vel,2)/accl)); + array.push_back(start_value + direction *(vel*(t_ipo*i) - 0.5*pow(vel,2)/accl)); } // tv <= t <= te for(; i <= (steps_tv + steps_tb + steps_te + 1); i++) { - array.push_back(start_value + direction * (vel * (te-tb) - 0.5 * accl * pow(te-(i*params_.profile.t_ipo),2))); + array.push_back(start_value + direction * (vel * tv - 0.5 * accl * pow(te-(t_ipo*i),2))); } return array; @@ -151,7 +133,6 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat { CartesianControllerUtils ccu; std::vector linear_path, roll_path, pitch_path, yaw_path; - std::vector sortedMatrix; double roll_start, pitch_start, yaw_start; //Convert to RPY @@ -166,25 +147,21 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); - // Sort the Matrix from the largest Se (0) to the smallest one (3) - sortedMatrix = pm.getSortedMatrix(); - // Get the profile timings from the longest path - pt_max_ = getMaxProfileTimings(sortedMatrix[0].Se_, params_.profile.accl, params_.profile.vel); + pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel); // Calculate the paths - for(int i = 0; i < sortedMatrix.size(); i++) + for(int i=0; i sqrt(std::fabs(Se) * accl / 2)) + { + vel = sqrt(std::fabs(Se) * accl / 2); + } - tb = 2 * vel / accl; - te = te_max + tb; + //TODO: Find a value. + if(vel > 0.001) + { + tb = utils.roundUpToMultiplier(2 * vel / accl, params_.profile.t_ipo); + if(te == 0) + { + te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb, params_.profile.t_ipo); + } tv = te - tb; + ROS_INFO_STREAM("========================================================================="); + ROS_INFO_STREAM("vel_new:" << vel << " accl: " << accl << " Se: " << std::fabs(Se)); + ROS_INFO_STREAM("tb:" << tb << " tv: " << tv << " te: " << te); + // Interpolationsteps for every timesequence - steps_tb = round(tb / params_.profile.t_ipo); - steps_tv = round((tv-tb) / params_.profile.t_ipo); - steps_te = round((te-tv) / params_.profile.t_ipo); - - // Reconfigure timings wtih t_ipo_ - pt.tb = steps_tb * params_.profile.t_ipo; - pt.tv = (steps_tb + steps_tv) * params_.profile.t_ipo; - pt.te = (steps_tb + steps_tv + steps_te) * params_.profile.t_ipo; - pt.steps_tb = steps_tb; - pt.steps_tv = steps_tv; - pt.steps_te = steps_te; - - return pt; -} + pt.tb = tb; + pt.tv = tv; + pt.te = te; -inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getMaxProfileTimings(double Se_max, double accl, double vel) -{ - // Calculate the Sinoid Profile Parameters - if (vel > sqrt(std::fabs(Se_max) * accl / 2)) + pt.steps_tb = pt.tb/params_.profile.t_ipo; + pt.steps_tv = (pt.tv-pt.tb)/params_.profile.t_ipo; + pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo; + + ROS_INFO_STREAM("pt.tb:" << pt.tb << " pt.tv: " << pt.tv << " pt.te: " << pt.te); + ROS_INFO_STREAM("pt.steps_tb:" << pt.steps_tb << " pt.steps_tv: " << pt.steps_tv << " pt.steps_te: " << pt.steps_te); + ROS_INFO_STREAM("========================================================================="); + + pt.ok = true; + } + else { - vel = sqrt(std::fabs(Se_max) * accl / 2); + pt.ok = false; } - return getProfileTimings((std::fabs(Se_max) / vel), accl, vel); // Calculate the velocity profile timings with respect to the largest Se + return pt; } + inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) { std::vector array; @@ -72,44 +86,11 @@ inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::Path double accl_max = params_.profile.accl; double vel_max = params_.profile.vel; - if(std::fabs(pa.Se_) > 0.001) + // Calculate the Profile Timings + pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max); + if(pt.ok) { - if(pa.calcTe_) - { - pt = pt_max_; - } - else - { - // Calculate the Profile Timings - vel_max = accl_max * pt_max_.te / 4 - sqrt((pow(accl_max,2)*pow(pt_max_.te,2)/16) - std::fabs(pa.Se_) * accl_max/2 ); - - pt = getProfileTimings(pt_max_.te, accl_max, vel_max); - } - array = getTrajectory(pa.start_value_ , pa.Se_, accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); - - switch(pa.idx_) - { - case 0: - { - ROS_INFO_STREAM("Linear-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); - break; - } - case 1: - { - ROS_INFO_STREAM("Roll-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) << " Acceleration: " << accl_max); - break; - } - case 2: - { - ROS_INFO_STREAM("Pitch-Path: " << pa.Se_ << std::setw(10) << std::left << " Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); - break; - } - case 3: - { - ROS_INFO_STREAM("Yaw-Path: " << pa.Se_ << std::setw(10) << std::left <<" Velocity: " << vel_max << std::setw(10) <<" Acceleration: " << accl_max); - break; - } - } + array = getTrajectory(pa.start_value_ , std::fabs(pa.Se_), accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } else { @@ -125,21 +106,24 @@ inline std::vector TrajectoryProfileSinoid::getTrajectory(double start_v std::vector array; unsigned int i = 1; double direction = se/std::fabs(se); + //Calculate the sinoid profile path - for(; i <= steps_tb ; i++) + // 0 <= t <= tb + for(; i <= steps_tb; i++) { array.push_back(start_value + direction * (accl*(0.25*pow(i*t_ipo,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*t_ipo))-1)))); } // tb <= t <= tv - for(; i <= (steps_tb + steps_tv) ; i++) + for(; i <= (steps_tb + steps_tv); i++) { array.push_back(start_value + direction * ( vel*(i*t_ipo-0.5*tb))); } // tv <= t <= te - for(; i <= (steps_tv + steps_tb + steps_te + 1) ; i++) + for(; i <= (steps_tv + steps_tb + steps_te + 1); i++) { array.push_back(start_value + direction * (0.5 * accl *(te*(i*t_ipo + tb) - 0.5*(pow(i*t_ipo,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/tb) * (i*t_ipo-tv)))))); } + return array; } @@ -149,7 +133,6 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m { CartesianControllerUtils ccu; std::vector linear_path, roll_path, pitch_path, yaw_path; - std::vector sortedMatrix; double roll_start, pitch_start, yaw_start; //Convert to RPY @@ -164,25 +147,21 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); - // Sort the Matrix from the largest Se (0) to the smallest one (3) - sortedMatrix = pm.getSortedMatrix(); - // Get the profile timings from the longest path - pt_max_ = getMaxProfileTimings(sortedMatrix[0].Se_, params_.profile.accl, params_.profile.vel); + pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel); // Calculate the paths - for(int i=0; igeneratePath(sortedMatrix[i]); + generatePath(pm.pm_[i]); } // Adjust the array length - ccu.adjustArrayLength(sortedMatrix); - - // Sort the arrays in the following order lin(0), roll(1), pitch(2), yaw(3) - ccu.sortMatrixByIdx(sortedMatrix); + // If no trajectory was interpolated, then this path array contains only one constant value. + // This constant value needs to be duplicated N_max times for matrix conversion purposes. + ccu.adjustArrayLength(pm.pm_); - ccu.copyMatrix(path_matrix,sortedMatrix); + ccu.copyMatrix(path_matrix,pm.pm_); return true; } From 435a8f89909d81aef88aa0d46a0661e4864d6c44 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 24 Nov 2015 18:13:34 +0100 Subject: [PATCH 09/35] Linear interpolation works fine now. There's still a bug in quaternion interpolation. --- .../cartesian_controller_data_types.h | 2 +- .../trajectory_profile_generator_base.h | 2 +- .../trajectory_profile_generator_ramp.h | 2 +- .../trajectory_profile_generator_sinoid.h | 2 +- .../trajectory_interpolator.cpp | 49 +++++++++++++------ .../trajectory_profile_generator_ramp.cpp | 24 ++++----- .../trajectory_profile_generator_sinoid.cpp | 18 +++---- cob_frame_tracker/src/cob_frame_tracker.cpp | 1 + 8 files changed, 61 insertions(+), 39 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index e1fcd13e..ea39a7b7 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -48,12 +48,12 @@ namespace cob_cartesian_controller double tb, te, tv; unsigned int steps_tb, steps_te, steps_tv; bool ok; + double vel; }; struct MoveLinStruct { geometry_msgs::Pose start, end; - bool rotate_only; }; struct MoveCircStruct diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 007039ea..e9ba515f 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -43,7 +43,7 @@ class TrajectoryProfileBase virtual ~TrajectoryProfileBase() {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel) = 0; + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) = 0; virtual bool calculateProfile(std::vector path_matrix[4], double Se, double Se_roll, double Se_pitch, double Se_yaw, geometry_msgs::Pose start) = 0; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index 05a183a5..28fa2f60 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -40,7 +40,7 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase { } ~TrajectoryProfileRamp() {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel); + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); virtual bool generatePath(cob_cartesian_controller::PathArray &pa); virtual bool calculateProfile(std::vector* path_matrix, double Se, double Se_roll, double Se_pitch, double Se_yaw, diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index b26f2422..7d1462f1 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -39,7 +39,7 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase { } ~TrajectoryProfileSinoid() {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel); + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); virtual bool generatePath(cob_cartesian_controller::PathArray &pa); virtual bool calculateProfile(std::vector* path_matrix, double Se, double Se_roll, double Se_pitch, double Se_yaw, diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index ab085046..7005e1d7 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -53,24 +53,45 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ // // Convert Quaternions into RPY Angles for start and end pose tf::quaternionMsgToTF(as.move_lin.start.orientation, q_start); -//// tf::Matrix3x3(q).getRPY(start_roll, start_pitch, start_yaw); -// + tf::Matrix3x3(q_start).getRPY(start_roll, start_pitch, start_yaw); + + ROS_INFO_STREAM("q_start_w: " << q_start.getW() << " q_start_x: " << q_start.getX() << " q_start_y: " << q_start.getY() << " q_start_z: " << q_start.getZ()); +// ROS_INFO_STREAM("start_roll: " << start_roll << " start_pitch: " << start_pitch << " start_yaw: " << start_yaw); + tf::quaternionMsgToTF(as.move_lin.end.orientation, q_end); -//// tf::Matrix3x3(q).getRPY(end_roll, end_pitch, end_yaw); -// q_relative = q_start * q_end.inverse(); -// q_relative.normalized(); -// -// q_relative = q_start * q_end.inverse(); + tf::Matrix3x3(q_end).getRPY(end_roll, end_pitch, end_yaw); + + ROS_INFO_STREAM("q_end_w: " << q_end.getW() << " q_end_x: " << q_end.getX() << " q_end_y: " << q_end.getY() << " q_end_z: " << q_end.getZ()); +// ROS_INFO_STREAM("end_roll: " << end_roll << " end_pitch: " << end_pitch << " end_yaw: " << end_yaw); + + +// q_relative = q_start.slerp(q_end, 1); +// ROS_INFO_STREAM("q_slerp_w: " << q_relative.getW() << " q_slerp_x: " << q_relative.getX() << " q_slerp_y: " << q_relative.getY() << " q_slerp_z: " << q_relative.getZ()); + + q_start = q_start.normalized(); + q_end = q_end.normalize(); + q_relative = q_end * q_start.inverse(); + + tf::Matrix3x3(q_relative).getRPY(Se_roll, Se_pitch, Se_yaw); + +// if(q_start.dot(q_end) > 1-0.0001) +// { +// Se_pitch = Se_roll = Se_yaw = 0; +// } +// q_relative = q_end * q_start.inverse(); // tf::Matrix3x3(q_relative).getRPY(Se_roll, Se_pitch, Se_yaw); +// ROS_INFO_STREAM("q_relative_w: " << q_relative.getW() << " q_relative_x: " << q_relative.getX() << " q_relative_y: " << q_relative.getY() << " q_relative_z: " << q_relative.getZ()); +// ROS_INFO_STREAM("Se_roll: " << Se_roll << " Se_pitch: " << Se_pitch << " Se_yaw: " << Se_yaw); + // Calculate path length for the angular movement - Se_roll = std::fabs(end_roll) - std::fabs(start_roll); - Se_pitch = std::fabs(end_pitch) - std::fabs(start_pitch); - Se_yaw = std::fabs(end_yaw) - std::fabs(start_yaw); + // Se_roll = std::fabs(end_roll) - std::fabs(start_roll); + // Se_pitch = std::fabs(end_pitch) - std::fabs(start_pitch); + // Se_yaw = std::fabs(end_yaw) - std::fabs(start_yaw); -// ROS_INFO_STREAM("Q_x: " << Se_roll << -// " Q_y: " << Se_pitch << -// " Q_z: " << Se_yaw); +// Se_roll = end_roll - start_roll; +// Se_pitch = end_pitch - start_pitch; +// Se_yaw = end_yaw - start_yaw; // Calculate path for each Angle if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, Se_roll, Se_pitch, Se_yaw, as.move_lin.start)) @@ -94,10 +115,10 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ } else { - ROS_INFO_STREAM("linear.path.at(..) = " << linear_path.at(i)); pose.position.x = as.move_lin.start.position.x + linear_path.at(i) * (as.move_lin.end.position.x - as.move_lin.start.position.x) / linear_path.back(); pose.position.y = as.move_lin.start.position.y + linear_path.at(i) * (as.move_lin.end.position.y - as.move_lin.start.position.y) / linear_path.back(); pose.position.z = as.move_lin.start.position.z + linear_path.at(i) * (as.move_lin.end.position.z - as.move_lin.start.position.z) / linear_path.back(); + } // Transform RPY to Quaternion diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index e90a0ab4..ff394a20 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -28,33 +28,29 @@ #include "ros/ros.h" #include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" /* BEGIN TrajectoryProfileRamp ********************************************************************************************/ -inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfileTimings(double Se, double te, double accl, double vel) +inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) { CartesianControllerUtils utils; cob_cartesian_controller::ProfileTimings pt; int steps_te, steps_tv, steps_tb = 0; double tv, tb = 0.0; +// ROS_WARN("Se_ ... : %f", Se); // Calculate the Sinoid Profile Parameters if (vel > sqrt(std::fabs(Se) * accl)) { vel = sqrt(std::fabs(Se) * accl); } - //Todo: Find a value. - if(vel > 0.001) + if(vel > 0.00001) { tb = utils.roundUpToMultiplier(vel / accl, params_.profile.t_ipo); - if(te == 0) + if(calcMaxTe) { te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb, params_.profile.t_ipo); } tv = te - tb; - ROS_INFO_STREAM("========================================================================="); - ROS_INFO_STREAM("vel_new:" << vel << " accl: " << accl << " Se: " << std::fabs(Se)); - ROS_INFO_STREAM("tb:" << tb << " tv: " << tv << " te: " << te); - // Interpolationsteps for every timesequence pt.tb = tb; pt.tv = tv; @@ -64,6 +60,10 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil pt.steps_tv = (pt.tv-pt.tb)/params_.profile.t_ipo; pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo; + pt.vel = vel; + + ROS_INFO_STREAM("========================================================================="); + ROS_INFO_STREAM("vel_new:" << vel << " accl: " << accl << " Se: " << std::fabs(Se)); ROS_INFO_STREAM("pt.tb:" << pt.tb << " pt.tv: " << pt.tv << " pt.te: " << pt.te); ROS_INFO_STREAM("pt.steps_tb:" << pt.steps_tb << " pt.steps_tv: " << pt.steps_tv << " pt.steps_te: " << pt.steps_te); ROS_INFO_STREAM("========================================================================="); @@ -87,10 +87,10 @@ inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathAr double vel_max = params_.profile.vel; // Calculate the Profile Timings - pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max); + pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); if(pt.ok) { - array = getTrajectory(pa.start_value_ , std::fabs(pa.Se_), accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); + array = getTrajectory(pa.start_value_ , pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } else { @@ -119,7 +119,7 @@ inline std::vector TrajectoryProfileRamp::getTrajectory(double start_val array.push_back(start_value + direction *(vel*(t_ipo*i) - 0.5*pow(vel,2)/accl)); } // tv <= t <= te - for(; i <= (steps_tv + steps_tb + steps_te + 1); i++) + for(; i <= (steps_tb + steps_tv + steps_te + 1); i++) { array.push_back(start_value + direction * (vel * tv - 0.5 * accl * pow(te-(t_ipo*i),2))); } @@ -148,7 +148,7 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); // Get the profile timings from the longest path - pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel); + pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); // Calculate the paths for(int i=0; i 0.001) { tb = utils.roundUpToMultiplier(2 * vel / accl, params_.profile.t_ipo); + if(te == 0) { te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb, params_.profile.t_ipo); } tv = te - tb; - ROS_INFO_STREAM("========================================================================="); - ROS_INFO_STREAM("vel_new:" << vel << " accl: " << accl << " Se: " << std::fabs(Se)); - ROS_INFO_STREAM("tb:" << tb << " tv: " << tv << " te: " << te); - // Interpolationsteps for every timesequence pt.tb = tb; pt.tv = tv; @@ -64,6 +60,10 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf pt.steps_tv = (pt.tv-pt.tb)/params_.profile.t_ipo; pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo; + pt.vel = vel; + + ROS_INFO_STREAM("========================================================================="); + ROS_INFO_STREAM("vel_new:" << vel << " accl: " << accl << " Se: " << std::fabs(Se)); ROS_INFO_STREAM("pt.tb:" << pt.tb << " pt.tv: " << pt.tv << " pt.te: " << pt.te); ROS_INFO_STREAM("pt.steps_tb:" << pt.steps_tb << " pt.steps_tv: " << pt.steps_tv << " pt.steps_te: " << pt.steps_te); ROS_INFO_STREAM("========================================================================="); @@ -87,10 +87,10 @@ inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::Path double vel_max = params_.profile.vel; // Calculate the Profile Timings - pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max); + pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); if(pt.ok) { - array = getTrajectory(pa.start_value_ , std::fabs(pa.Se_), accl_max, vel_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); + array = getTrajectory(pa.start_value_ , pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } else { @@ -148,7 +148,7 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); // Get the profile timings from the longest path - pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel); + pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); // Calculate the paths for(int i=0; i= max_vel_lin_) From e3efaba9ad577f3b77505f403e60fdb0b524fa31 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 1 Dec 2015 14:26:25 +0100 Subject: [PATCH 10/35] New quaternion interpolation --- .../cartesian_controller_data_types.h | 16 +- .../trajectory_profile_generator_base.h | 9 +- .../trajectory_profile_generator_ramp.h | 8 +- .../trajectory_profile_generator_sinoid.h | 5 +- .../src/cartesian_controller.cpp | 155 ++++++++++-------- .../trajectory_interpolator.cpp | 130 +++++---------- .../trajectory_profile_generator_ramp.cpp | 35 ++-- .../trajectory_profile_generator_sinoid.cpp | 31 ++-- 8 files changed, 176 insertions(+), 213 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index ea39a7b7..01e7cdc7 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -76,12 +76,10 @@ namespace cob_cartesian_controller class PathArray{ public: - PathArray(double idx, double Se, double start_value, std::vector array): + PathArray(double Se, std::vector array): Se_(Se), - array_(array), - start_value_(start_value) + array_(array) { - idx_= idx; calcTe_ = false; } @@ -90,25 +88,19 @@ namespace cob_cartesian_controller array_.clear(); } - unsigned int idx_; bool calcTe_; double Se_; std::vector array_; - double start_value_; }; class PathMatrix{ public: PathMatrix(PathArray &pa1, - PathArray &pa2, - PathArray &pa3, - PathArray &pa4) + PathArray &pa2) { pm_.push_back(pa1); pm_.push_back(pa2); - pm_.push_back(pa3); - pm_.push_back(pa4); } ~PathMatrix() @@ -124,7 +116,7 @@ namespace cob_cartesian_controller { double se_max = 0; - for(int i=0; i<4; i++) + for(int i=0; i path_matrix[4], - double Se, double Se_roll, double Se_pitch, double Se_yaw, + + virtual bool calculateProfile(std::vector* path_matrix, + const double Se_lin, const double Se_rot, geometry_msgs::Pose start) = 0; - virtual std::vector getTrajectory(double start_value, double se, - double accl, double vel, double t_ipo, + + virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; private: virtual bool generatePath(cob_cartesian_controller::PathArray &pa) = 0; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index 28fa2f60..90491ed4 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -42,15 +42,13 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase ~TrajectoryProfileRamp() {} virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); virtual bool generatePath(cob_cartesian_controller::PathArray &pa); - virtual bool calculateProfile(std::vector* path_matrix, - double Se, double Se_roll, double Se_pitch, double Se_yaw, + virtual bool calculateProfile(std::vector *path_matrix, + const double Se_lin, const double Se_rot, geometry_msgs::Pose start); - virtual std::vector getTrajectory(double start_value, double se, - double accl, double vel, double t_ipo, + virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); private: -// virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel); cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileRamp **********************************************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 7d1462f1..f48062dc 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -42,10 +42,9 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); virtual bool generatePath(cob_cartesian_controller::PathArray &pa); virtual bool calculateProfile(std::vector* path_matrix, - double Se, double Se_roll, double Se_pitch, double Se_yaw, + const double Se_lin, const double Se_rot, geometry_msgs::Pose start); - virtual std::vector getTrajectory(double start_value, double se, - double accl, double vel, double t_ipo, + virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); private: diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index 9a8c5fe3..f15aeca0 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -151,6 +151,7 @@ bool CartesianController::stopTracking() // MovePTP bool CartesianController::movePTP(const geometry_msgs::Pose& target_pose, const double epsilon) { + ROS_WARN("TEST"); bool success = false; int reached_pos_counter = 0; @@ -218,32 +219,64 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca cartesian_path.poses.at(i).orientation.y, cartesian_path.poses.at(i).orientation.z, cartesian_path.poses.at(i).orientation.w) ); + +// geometry_msgs::Transform tf_msg; +// tf::transformTFToMsg(transform, tf_msg); +// ROS_INFO_STREAM("pathArray[" << i << "]: " << tf_msg.rotation); tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); + // Get transformation tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); // Check whether chain_tip_link is within epsilon area of target_frame - if(!utils_.inEpsilonArea(stamped_transform, epsilon)) - { - failure_counter++; - } - else - { - if(failure_counter > 0) - { - failure_counter--; - } - } +// if(!utils_.inEpsilonArea(stamped_transform, epsilon)) +// { +// failure_counter++; +// } +// else +// { +// if(failure_counter > 0) +// { +// failure_counter--; +// } +// } //ToDo: This functionality is already implemented in the frame_tracker //Use the the ActionInterface of the FrameTracker instead of the ServiceCalls and modify constraints in FrameTracker accordingly - if(failure_counter > 100) +// if(failure_counter > 100) +// { +// ROS_ERROR("Endeffector failed to track target_frame!"); +// success = false; +// break; +// } + ros::spinOnce(); + rate.sleep(); + } + + while(true) + { + // Get transformation + tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); + ROS_INFO_STREAM("stamped: x: " << stamped_transform.getOrigin().getX() << " y: " << stamped_transform.getOrigin().getY() << " z: " << stamped_transform.getOrigin().getZ()); + + if(!utils_.inEpsilonArea(stamped_transform, 0.0005)) + { + // Send/Refresh target Frame + transform.setOrigin( tf::Vector3(cartesian_path.poses.back().position.x, + cartesian_path.poses.back().position.y, + cartesian_path.poses.back().position.z) ); + transform.setRotation( tf::Quaternion(cartesian_path.poses.back().orientation.x, + cartesian_path.poses.back().orientation.y, + cartesian_path.poses.back().orientation.z, + cartesian_path.poses.back().orientation.w) ); + tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); + } + else { - ROS_ERROR("Endeffector failed to track target_frame!"); - success = false; break; } + ros::spinOnce(); rate.sleep(); } @@ -264,7 +297,6 @@ void CartesianController::goalCallback() ROS_INFO_STREAM("========================================================================="); start_pose = utils_.getPose(root_frame_, chain_tip_link_); - ROS_INFO_STREAM("Start Pose\n X: " << start_pose.position.x << ", Y:" << start_pose.position.y << " , Z: " << start_pose.position.z); action_struct = acceptGoal(as_->acceptNewGoal()); @@ -280,8 +312,6 @@ void CartesianController::goalCallback() } // Publish Preview -// visualization_msgs::MarkerArray marker_array; -// utils_.previewPath(cartesian_path,marker_array); utils_.previewPath(cartesian_path); // Activate Tracking @@ -298,13 +328,7 @@ void CartesianController::goalCallback() return; } -// movePTP(action_struct.move_lin.end, 0.005); - -// end_pose = utils_.getPose(root_frame_,chain_tip_link_); -// -// ROS_INFO_STREAM("Relative \n X: " << fabs(end_pose.position.x) - fabs(action_struct.move_lin.end.position.x) << -// ", Y:" << fabs(end_pose.position.y) - fabs(action_struct.move_lin.end.position.y) << -// " , Z: " << fabs(end_pose.position.z) - fabs(action_struct.move_lin.end.position.z)); + movePTP(action_struct.move_lin.end, 0.001); // De-Activate Tracking if(!stopTracking()) @@ -318,45 +342,45 @@ void CartesianController::goalCallback() else if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::CIRC) { ROS_INFO("move_circ"); - - if(!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct)) - { - actionAbort(false, "Failed to do interpolation for 'move_circ'"); - return; - } - - // Publish Preview - utils_.previewPath(cartesian_path); - - // Activate Tracking - if(!startTracking()) - { - actionAbort(false, "Failed to start tracking"); - return; - } - - // Move to start - if(!movePTP(cartesian_path.poses.at(0), 0.03)) - { - actionAbort(false, "Failed to movePTP to start"); - return; - } - - // Execute path - if(!posePathBroadcaster(cartesian_path)) - { - actionAbort(false, "Failed to execute path for 'move_circ'"); - return; - } - - // De-Activate Tracking - if(!stopTracking()) - { - actionAbort(false, "Failed to stop tracking"); - return; - } - - actionSuccess(true, "move_circ succeeded!"); +// if(!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct)) +// { +// actionAbort(false, "Failed to do interpolation for 'move_circ'"); +// return; +// } +// +// // Publish Preview +// utils_.previewPath(cartesian_path); +// +// // Activate Tracking +// if(!startTracking()) +// { +// actionAbort(false, "Failed to start tracking"); +// return; +// } +// +// // Move to start +// if(!movePTP(cartesian_path.poses.at(0), 0.03)) +// { +// actionAbort(false, "Failed to movePTP to start"); +// return; +// } +// +// // Execute path +// if(!posePathBroadcaster(cartesian_path)) +// { +// actionAbort(false, "Failed to execute path for 'move_circ'"); +// return; +// } +// +// // De-Activate Tracking +// if(!stopTracking()) +// { +// actionAbort(false, "Failed to stop tracking"); +// return; +// } +// +// actionSuccess(true, "move_circ succeeded!"); + actionAbort(false, "Not implemented."); } else { @@ -372,11 +396,14 @@ cob_cartesian_controller::MoveLinStruct CartesianController::convertMoveLin(cons utils_.transformPose(move_lin_msg.frame_id, root_frame_, move_lin_msg.pose_goal, end); cob_cartesian_controller::MoveLinStruct move_lin; - move_lin.rotate_only = move_lin_msg.rotate_only; move_lin.start = start; move_lin.end = end; + ROS_WARN("move_lin_msg_orientation .. w: %f", move_lin_msg.pose_goal.orientation.w); + ROS_WARN("move_lin_msg_orientation .. x: %f", move_lin_msg.pose_goal.orientation.x); + ROS_WARN("move_lin_msg_orientation .. y: %f", move_lin_msg.pose_goal.orientation.y); + ROS_WARN("move_lin_msg_orientation .. z: %f", move_lin_msg.pose_goal.orientation.z); return move_lin; } diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 7005e1d7..14d779da 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -33,76 +33,49 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_array, const cob_cartesian_controller::CartesianActionStruct& as) { - double Se_roll, Se_pitch, Se_yaw; this->trajectory_profile_generator_.reset(TrajectoryProfileBuilder::createProfile(as)); pose_array.header.stamp = ros::Time::now(); pose_array.header.frame_id = root_frame_; - tf::Quaternion q_start, q_end, q_relative, q; - double start_roll, start_pitch, start_yaw; - double end_roll, end_pitch, end_yaw; + tf::Quaternion q_start, q_end; - std::vector linear_path, roll_path, pitch_path, yaw_path; - std::vector path_matrix[4]; + std::vector linear_path, angular_path, path; + std::vector path_matrix[2]; geometry_msgs::Pose pose; - double Se = sqrt(pow((as.move_lin.end.position.x - as.move_lin.start.position.x), 2) + + double norm_factor; + tf::quaternionMsgToTF(as.move_lin.start.orientation,q_start); + tf::quaternionMsgToTF(as.move_lin.end.orientation,q_end); + + double Se_lin = sqrt(pow((as.move_lin.end.position.x - as.move_lin.start.position.x), 2) + pow((as.move_lin.end.position.y - as.move_lin.start.position.y), 2) + pow((as.move_lin.end.position.z - as.move_lin.start.position.z), 2)); -// // Convert Quaternions into RPY Angles for start and end pose - tf::quaternionMsgToTF(as.move_lin.start.orientation, q_start); - tf::Matrix3x3(q_start).getRPY(start_roll, start_pitch, start_yaw); - - ROS_INFO_STREAM("q_start_w: " << q_start.getW() << " q_start_x: " << q_start.getX() << " q_start_y: " << q_start.getY() << " q_start_z: " << q_start.getZ()); -// ROS_INFO_STREAM("start_roll: " << start_roll << " start_pitch: " << start_pitch << " start_yaw: " << start_yaw); - - tf::quaternionMsgToTF(as.move_lin.end.orientation, q_end); - tf::Matrix3x3(q_end).getRPY(end_roll, end_pitch, end_yaw); - - ROS_INFO_STREAM("q_end_w: " << q_end.getW() << " q_end_x: " << q_end.getX() << " q_end_y: " << q_end.getY() << " q_end_z: " << q_end.getZ()); -// ROS_INFO_STREAM("end_roll: " << end_roll << " end_pitch: " << end_pitch << " end_yaw: " << end_yaw); - - -// q_relative = q_start.slerp(q_end, 1); -// ROS_INFO_STREAM("q_slerp_w: " << q_relative.getW() << " q_slerp_x: " << q_relative.getX() << " q_slerp_y: " << q_relative.getY() << " q_slerp_z: " << q_relative.getZ()); - - q_start = q_start.normalized(); - q_end = q_end.normalize(); - q_relative = q_end * q_start.inverse(); - - tf::Matrix3x3(q_relative).getRPY(Se_roll, Se_pitch, Se_yaw); - -// if(q_start.dot(q_end) > 1-0.0001) -// { -// Se_pitch = Se_roll = Se_yaw = 0; -// } -// q_relative = q_end * q_start.inverse(); -// tf::Matrix3x3(q_relative).getRPY(Se_roll, Se_pitch, Se_yaw); - -// ROS_INFO_STREAM("q_relative_w: " << q_relative.getW() << " q_relative_x: " << q_relative.getX() << " q_relative_y: " << q_relative.getY() << " q_relative_z: " << q_relative.getZ()); -// ROS_INFO_STREAM("Se_roll: " << Se_roll << " Se_pitch: " << Se_pitch << " Se_yaw: " << Se_yaw); - - // Calculate path length for the angular movement - // Se_roll = std::fabs(end_roll) - std::fabs(start_roll); - // Se_pitch = std::fabs(end_pitch) - std::fabs(start_pitch); - // Se_yaw = std::fabs(end_yaw) - std::fabs(start_yaw); - -// Se_roll = end_roll - start_roll; -// Se_pitch = end_pitch - start_pitch; -// Se_yaw = end_yaw - start_yaw; + double Se_rot = q_start.angleShortestPath(q_end); + ROS_INFO_STREAM("Se_lin: " << Se_lin << " Se_rot: " << Se_rot); // Calculate path for each Angle - if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, Se_roll, Se_pitch, Se_yaw, as.move_lin.start)) +// (std::vector path_matrix[2],const double Se_lin, const double Se_rot,geometry_msgs::Pose start) + + if(!trajectory_profile_generator_->calculateProfile(path_matrix, Se_lin, Se_rot, as.move_lin.start)) { return false; } linear_path = path_matrix[0]; - roll_path = path_matrix[1]; - pitch_path = path_matrix[2]; - yaw_path = path_matrix[3]; + angular_path = path_matrix[1]; + + if(linear_path.back() == 0) + { + norm_factor = 1/angular_path.back(); + path = angular_path; + } + else + { + norm_factor = 1/linear_path.back(); + path = linear_path; + } // Interpolate the linear path for(unsigned int i = 0 ; i < path_matrix[0].size() ; i++) @@ -118,13 +91,9 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ pose.position.x = as.move_lin.start.position.x + linear_path.at(i) * (as.move_lin.end.position.x - as.move_lin.start.position.x) / linear_path.back(); pose.position.y = as.move_lin.start.position.y + linear_path.at(i) * (as.move_lin.end.position.y - as.move_lin.start.position.y) / linear_path.back(); pose.position.z = as.move_lin.start.position.z + linear_path.at(i) * (as.move_lin.end.position.z - as.move_lin.start.position.z) / linear_path.back(); - } - // Transform RPY to Quaternion - q.setRPY(roll_path.at(i), pitch_path.at(i), yaw_path.at(i)); - tf::quaternionTFToMsg(q, pose.orientation); - + tf::quaternionTFToMsg(q_start.slerp(q_end, path.at(i) * norm_factor), pose.orientation); pose_array.poses.push_back(pose); } return true; @@ -138,56 +107,47 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos pose_array.header.stamp = ros::Time::now(); pose_array.header.frame_id = root_frame_; + tf::Quaternion q_start, q_end; tf::Quaternion q; tf::Transform C, P, T; + double norm_factor; + tf::quaternionMsgToTF(as.move_lin.start.orientation,q_start); + tf::quaternionMsgToTF(as.move_lin.end.orientation,q_end); + + std::vector linear_path, angular_path, path; + std::vector path_array; - std::vector path_matrix[4]; + std::vector path_matrix[2]; geometry_msgs::Pose pose; - double Se = as.move_circ.end_angle - as.move_circ.start_angle; + double Se_rot = as.move_circ.end_angle - as.move_circ.start_angle; - bool forward; - if(Se < 0) - { - forward = false; - } - else - { - forward = true; - } - Se = std::fabs(Se); + Se_rot = std::fabs(Se_rot); // Calculates the Path with RAMP or SINOID profile - if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0, 0, 0, pose)) + if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se_rot, 0, pose)) { return false; } - path_array = path_matrix[0]; + linear_path = path_matrix[0]; + angular_path = path_matrix[1]; + // Define Center Pose C.setOrigin(tf::Vector3(as.move_circ.pose_center.position.x, as.move_circ.pose_center.position.y, as.move_circ.pose_center.position.z)); tf::quaternionMsgToTF(as.move_circ.pose_center.orientation, q); C.setRotation(q); // Interpolate the circular path - for(int i = 0 ; i < path_array.size() ; i++) + for(int i = 0 ; i < linear_path.size() ; i++) { - // Rotate T - T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); - - if(forward) - { - T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); - q.setRPY(0, -path_array.at(i), 0); - } - else - { - T.setOrigin(tf::Vector3(cos(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius, 0, sin(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius)); - q.setRPY(0, path_array.at(i), 0); - } + ROS_INFO_STREAM("linear_path.at(" << i << "): " << linear_path.at(i)); + // Rotate T + T.setOrigin(tf::Vector3(cos(linear_path.at(i)) * as.move_circ.radius, 0, sin(linear_path.at(i)) * as.move_circ.radius)); + q.setRPY(0, path_array.at(i), 0); T.setRotation(q); // Calculate TCP Position diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index ff394a20..47cdef12 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -35,7 +35,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil int steps_te, steps_tv, steps_tb = 0; double tv, tb = 0.0; -// ROS_WARN("Se_ ... : %f", Se); // Calculate the Sinoid Profile Parameters if (vel > sqrt(std::fabs(Se) * accl)) { @@ -90,18 +89,18 @@ inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathAr pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); if(pt.ok) { - array = getTrajectory(pa.start_value_ , pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); + array = getTrajectory(pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } else { - array.push_back(pa.start_value_); + array.push_back(0); } pa.array_ = array; return true; } -inline std::vector TrajectoryProfileRamp::getTrajectory(double start_value, double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) +inline std::vector TrajectoryProfileRamp::getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) { std::vector array; unsigned int i = 1; @@ -111,47 +110,41 @@ inline std::vector TrajectoryProfileRamp::getTrajectory(double start_val // 0 <= t <= tb for(; i <= steps_tb ; i++) { - array.push_back(start_value + direction * (0.5*accl*pow((t_ipo*i),2))); + array.push_back(direction * (0.5*accl*pow((t_ipo*i),2))); } // tb <= t <= tv for(; i <= (steps_tb + steps_tv); i++) { - array.push_back(start_value + direction *(vel*(t_ipo*i) - 0.5*pow(vel,2)/accl)); + array.push_back(direction *(vel*(t_ipo*i) - 0.5*pow(vel,2)/accl)); } // tv <= t <= te for(; i <= (steps_tb + steps_tv + steps_te + 1); i++) { - array.push_back(start_value + direction * (vel * tv - 0.5 * accl * pow(te-(t_ipo*i),2))); + array.push_back(direction * (vel * tv - 0.5 * accl * pow(te-(t_ipo*i),2))); } return array; } -inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_matrix[4], - const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, +inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_matrix[2], + const double Se_lin, const double Se_rot, geometry_msgs::Pose start) { CartesianControllerUtils ccu; - std::vector linear_path, roll_path, pitch_path, yaw_path; - double roll_start, pitch_start, yaw_start; + std::vector linear_path, angular_path; - //Convert to RPY - tf::Quaternion q; - tf::quaternionMsgToTF(start.orientation, q); - tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); + cob_cartesian_controller::PathArray lin(Se_lin, linear_path); + cob_cartesian_controller::PathArray rot(Se_rot, angular_path); - cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); - cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); - cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); - cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); - cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); + cob_cartesian_controller::PathMatrix pm(lin,rot); // Get the profile timings from the longest path pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); + ROS_INFO_STREAM("pt_max_SEEEEEEEEEEEEE: " << pm.getMaxSe()); // Calculate the paths - for(int i=0; i TrajectoryProfileSinoid::getTrajectory(double start_value, double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) +inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) { std::vector array; unsigned int i = 1; @@ -111,41 +111,34 @@ inline std::vector TrajectoryProfileSinoid::getTrajectory(double start_v // 0 <= t <= tb for(; i <= steps_tb; i++) { - array.push_back(start_value + direction * (accl*(0.25*pow(i*t_ipo,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*t_ipo))-1)))); + array.push_back(direction * (accl*(0.25*pow(i*t_ipo,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*t_ipo))-1)))); } // tb <= t <= tv for(; i <= (steps_tb + steps_tv); i++) { - array.push_back(start_value + direction * ( vel*(i*t_ipo-0.5*tb))); + array.push_back(direction * ( vel*(i*t_ipo-0.5*tb))); } // tv <= t <= te for(; i <= (steps_tv + steps_tb + steps_te + 1); i++) { - array.push_back(start_value + direction * (0.5 * accl *(te*(i*t_ipo + tb) - 0.5*(pow(i*t_ipo,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/tb) * (i*t_ipo-tv)))))); + array.push_back(direction * (0.5 * accl *(te*(i*t_ipo + tb) - 0.5*(pow(i*t_ipo,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/tb) * (i*t_ipo-tv)))))); } return array; } -inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_matrix[4], - const double Se, const double Se_roll, const double Se_pitch, const double Se_yaw, +inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_matrix[2], + const double Se_lin, const double Se_rot, geometry_msgs::Pose start) { CartesianControllerUtils ccu; - std::vector linear_path, roll_path, pitch_path, yaw_path; - double roll_start, pitch_start, yaw_start; + std::vector linear_path, angular_path; - //Convert to RPY - tf::Quaternion q; - tf::quaternionMsgToTF(start.orientation, q); - tf::Matrix3x3(q).getRPY(roll_start, pitch_start, yaw_start); + cob_cartesian_controller::PathArray lin(Se_lin, linear_path); + cob_cartesian_controller::PathArray rot(Se_rot, angular_path); - cob_cartesian_controller::PathArray lin(0, Se, 0, linear_path); - cob_cartesian_controller::PathArray roll(1, Se_roll, roll_start, roll_path); - cob_cartesian_controller::PathArray pitch(2, Se_pitch, pitch_start, pitch_path); - cob_cartesian_controller::PathArray yaw(3, Se_yaw, yaw_start, yaw_path); - cob_cartesian_controller::PathMatrix pm(lin,roll,pitch,yaw); + cob_cartesian_controller::PathMatrix pm(lin,rot); // Get the profile timings from the longest path pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); From 90eb9baa21a0ebad7239d7eb9eca7049afa99cd2 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 1 Dec 2015 14:27:31 +0100 Subject: [PATCH 11/35] LWA4D test scripts --- .../scripts/lwa4d/lwa4d_circle.py | 66 +++++++++ .../scripts/lwa4d/lwa4d_quad.py | 139 ++++++++++++++++++ .../scripts/lwa4d/lwa4d_quad_angular.py | 121 +++++++++++++++ 3 files changed, 326 insertions(+) create mode 100755 cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py create mode 100755 cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py create mode 100755 cob_cartesian_controller/scripts/lwa4d/lwa4d_quad_angular.py diff --git a/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py b/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py new file mode 100755 index 00000000..6a23270b --- /dev/null +++ b/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py @@ -0,0 +1,66 @@ +#! /usr/bin/env python +import math +import rospy + +from geometry_msgs.msg import Pose +from cob_cartesian_controller.msg import Profile +from simple_script_server.simple_script_server import simple_script_server +import simple_cartesian_interface as sci + + +def init_pos(): + sss = simple_script_server() + sss.move("arm", [[-0.0002934322105607734, -0.38304632633953606, 6.931483707006691e-07, 0.8526320037121202, 5.69952198326007e-07, -0.47039657856235184, -0.00029228225570943067]]) + +if __name__ == '__main__': + rospy.init_node('test_move_circ_interface') + init_pos() + + + pose = sci.gen_pose(pos=[-0.2, 0, 0.8], rpy=[0, 0.0, 0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.1 + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + + + pose = sci.gen_pose(pos=[0.0, 0.0, 0.8], rpy=[0.0, 0.0, 0]) + start_angle = 180.0 * math.pi / 180.0 + end_angle = 0.0 * math.pi / 180.0 + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.1 + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP + + success, message = sci.move_circ(pose, "world", start_angle, end_angle, 0.2, profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + + pose = sci.gen_pose(pos=[0, 0, 1], rpy=[0, 0.0, -math.pi/2]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.1 + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(3.0) diff --git a/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py b/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py new file mode 100755 index 00000000..24d896e7 --- /dev/null +++ b/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py @@ -0,0 +1,139 @@ +#! /usr/bin/env python +import math +import rospy + +from geometry_msgs.msg import Pose +from cob_cartesian_controller.msg import Profile +from simple_script_server.simple_script_server import simple_script_server +import simple_cartesian_interface as sci + + +def init_pos(): + sss = simple_script_server() + sss.move("arm", [[-0.00032004093963244884, -0.7064191894021441, -1.577532922958369e-06, 1.4183686971944311, 1.2084352562169443e-05, -0.6913530502577565, -0.0002663056533762642]]) + +if __name__ == '__main__': + rospy.init_node('test_move_lin_interface') + init_pos() + + + pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0.0, 0.0, 0.0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + + rospy.sleep(3.0) + + pose = sci.gen_pose(pos=[0.3, 0, 0.9], rpy=[0.0, 0.0, 90.0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + + rospy.sleep(1.0) + + pose = sci.gen_pose(pos=[0.3, 0, 0.8], rpy=[0.0, -0.2, 0.5]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + + rospy.sleep(1.0) + + + pose = sci.gen_pose(pos=[-0.3, 0, 0.8], rpy=[0.0, 1.0 , -0.5]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + + rospy.sleep(1.0) + + pose = sci.gen_pose(pos=[-0.3, 0, 0.9], rpy=[0.0, 0.0, 0.0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(1.0) + + + + pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0.0, 0.0, 0.0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(1.0) + + + + pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0.5, 0.0, 0.0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(1.0) diff --git a/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad_angular.py b/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad_angular.py new file mode 100755 index 00000000..9db51481 --- /dev/null +++ b/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad_angular.py @@ -0,0 +1,121 @@ +#! /usr/bin/env python +import math +import rospy + +from geometry_msgs.msg import Pose +from cob_cartesian_controller.msg import Profile +from simple_script_server.simple_script_server import simple_script_server +import simple_cartesian_interface as sci + + +def init_pos(): + sss = simple_script_server() + sss.move("arm", [[-0.00032004093963244884, -0.7064191894021441, -1.577532922958369e-06, 1.4183686971944311, 1.2084352562169443e-05, -0.6913530502577565, -0.0002663056533762642]]) + +if __name__ == '__main__': + rospy.init_node('test_move_lin_interface') + init_pos() + + + pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0, 0.0, 0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(3.0) + + + pose = sci.gen_pose(pos=[0.3, 0, 0.9], rpy=[0, 0.0, 0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(3.0) + + + pose = sci.gen_pose(pos=[0.3, 0, 0.8], rpy=[0, 0.0, 0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(3.0) + + + + pose = sci.gen_pose(pos=[-0.3, 0, 0.8], rpy=[0, 0.0, 0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(3.0) + + + pose = sci.gen_pose(pos=[-0.3, 0, 0.9], rpy=[0, 0.0, 0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(2.0) + + + + pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0, 0.0, 0]) + profile = Profile() + profile.vel = 0.1 + profile.accl = 0.05 + #profile.profile_type = Profile.SINOID + profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + rospy.sleep(3.0) From a044d07f442abc7be34ef506dc7840f6080e73d8 Mon Sep 17 00:00:00 2001 From: Marco Bezzon Date: Tue, 1 Dec 2015 14:50:28 +0100 Subject: [PATCH 12/35] adjust service type in cartesian controller --- cob_cartesian_controller/src/cartesian_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index f15aeca0..cf51d851 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include @@ -66,7 +66,7 @@ bool CartesianController::initialize() ROS_WARN("Waiting for Services..."); start_tracking_ = nh_.serviceClient("frame_tracker/start_tracking"); - stop_tracking_ = nh_.serviceClient("frame_tracker/stop_tracking"); + stop_tracking_ = nh_.serviceClient("frame_tracker/stop"); start_tracking_.waitForExistence(); stop_tracking_.waitForExistence(); tracking_ = false; @@ -124,7 +124,7 @@ bool CartesianController::startTracking() bool CartesianController::stopTracking() { bool success = false; - std_srvs::Empty stop; + std_srvs::Trigger stop; if(tracking_) { success = stop_tracking_.call(stop); From 43ced7eeb5ac70f8ffeddb3e3f208f2b660f4195 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 1 Dec 2015 15:58:12 +0100 Subject: [PATCH 13/35] remove obsolete files --- .../trajactory_profile_generator.h | 45 ------------- .../trajactory_profile_generator_base.h | 63 ------------------- .../trajectory_profile_generator.cpp | 55 ---------------- 3 files changed, 163 deletions(-) delete mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h delete mode 100644 cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h delete mode 100644 cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h deleted file mode 100644 index 6478a458..00000000 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h +++ /dev/null @@ -1,45 +0,0 @@ -/*! - ***************************************************************** - * \file - * - * \note - * Copyright (c) 2015 \n - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) \n\n - * - ***************************************************************** - * - * \note - * Project name: care-o-bot - * \note - * ROS stack name: cob_control - * \note - * ROS package name: cob_cartesian_controller - * - * \author - * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com - * - * \date Date of creation: September, 2015 - * - * \brief - * - ****************************************************************/ -#ifndef TRAJECTORY_PROFILE_BUILDER_H_ -#define TRAJECTORY_PROFILE_BUILDER_H_ - -#include "cob_cartesian_controller/cartesian_controller_data_types.h" -#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h" - - -/* BEGIN TrajectoryProfileBuilder *****************************************************************************************/ -class TrajectoryProfileBuilder -{ - public: - TrajectoryProfileBuilder() {} - ~TrajectoryProfileBuilder() {} - - static TrajectoryProfileBase* createProfile(const cob_cartesian_controller::CartesianActionStruct& params); -}; -/* END TrajectoryGeneratorBuilder *******************************************************************************************/ - -#endif diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h deleted file mode 100644 index 203bb8ca..00000000 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator_base.h +++ /dev/null @@ -1,63 +0,0 @@ -/*! - ***************************************************************** - * \file - * - * \note - * Copyright (c) 2015 \n - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) \n\n - * - ***************************************************************** - * - * \note - * Project name: care-o-bot - * \note - * ROS stack name: cob_control - * \note - * ROS package name: cob_twist_controller - * - * \author - * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com - * - * \date Date of creation: June, 2015 - * - * \brief - * This header contains the interface description of all available - * hardware interface types (position/velocity). - * - ****************************************************************/ -#ifndef HARDWARE_INTERFACE_TYPE_BASE_H_ -#define HARDWARE_INTERFACE_TYPE_BASE_H_ - -#include "ros/ros.h" -#include "cob_cartesian_controller/cartesian_controller_data_types.h" -#include - -/// Base class for hardware interfaces types. -class TrajectoryProfileBase -{ - public: - TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct& params): - params_(params) - {vel_max_=0;} - - virtual ~TrajectoryProfileBase() {} - - virtual cob_cartesian_controller::ProfileTimings getMaxProfileTimings(double Se_max, double accl, double vel) = 0; - virtual bool calculateProfile(std::vector path_matrix[4], - double Se, double Se_roll, double Se_pitch, double Se_yaw, - geometry_msgs::Pose start) = 0; - virtual std::vector getTrajectory(double start_value, double se, - double accl, double vel, double t_ipo, - double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; - private: - virtual bool generatePath(cob_cartesian_controller::PathArray &pa) = 0; - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel) = 0; - - - protected: - const cob_cartesian_controller::CartesianActionStruct ¶ms_; - double vel_max_; -}; - -#endif /* HARDWARE_INTERFACE_TYPE_BASE_H_ */ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp deleted file mode 100644 index 98b3f54d..00000000 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator.cpp +++ /dev/null @@ -1,55 +0,0 @@ - -/*! - ***************************************************************** - * \file - * - * \note - * Copyright (c) 2015 \n - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) \n\n - * - ***************************************************************** - * - * \note - * Project name: care-o-bot - * \note - * ROS stack name: cob_control - * \note - * ROS package name: cob_twist_controller - * - * \author - * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com - * - * \date Date of creation: June, 2015 - * - * \brief - * This module contains the implementation of all interface types. - * - ****************************************************************/ -#define RAMP_PROFILE 1 -#define SINOID_PROFILE 2 -#include "cob_cartesian_controller/trajectory_profile_generator/trajactory_profile_generator.h" -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h" - -/* BEGIN TrajectoryProfileBase *****************************************************************************************/ - -TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesian_controller::CartesianActionStruct& params) -{ - TrajectoryProfileBase* ib = NULL; - switch(params.profile.profile_type) - { - case RAMP_PROFILE: - ib = new TrajectoryProfileRamp(params); - break; - case SINOID_PROFILE: - ib = new TrajectoryProfileSinoid(params); - break; - default: - ROS_ERROR("Unknown Profile"); - break; - } - - return ib; -} -/* END TrajectoryProfileBase *******************************************************************************************/ From a4ecf06f5cd6f305847ca979f835370a863b8f71 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 1 Dec 2015 18:27:49 +0100 Subject: [PATCH 14/35] Implemented move_circ --- .../src/cartesian_controller.cpp | 98 +++++--------- .../simple_cartesian_interface.py | 2 + .../trajectory_interpolator.cpp | 122 +++++++++--------- .../trajectory_profile_generator_ramp.cpp | 7 - .../trajectory_profile_generator_sinoid.cpp | 7 - 5 files changed, 97 insertions(+), 139 deletions(-) diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index cf51d851..0b07d104 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -151,7 +151,6 @@ bool CartesianController::stopTracking() // MovePTP bool CartesianController::movePTP(const geometry_msgs::Pose& target_pose, const double epsilon) { - ROS_WARN("TEST"); bool success = false; int reached_pos_counter = 0; @@ -220,36 +219,11 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca cartesian_path.poses.at(i).orientation.z, cartesian_path.poses.at(i).orientation.w) ); -// geometry_msgs::Transform tf_msg; -// tf::transformTFToMsg(transform, tf_msg); -// ROS_INFO_STREAM("pathArray[" << i << "]: " << tf_msg.rotation); tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); - // Get transformation tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); - // Check whether chain_tip_link is within epsilon area of target_frame -// if(!utils_.inEpsilonArea(stamped_transform, epsilon)) -// { -// failure_counter++; -// } -// else -// { -// if(failure_counter > 0) -// { -// failure_counter--; -// } -// } - - //ToDo: This functionality is already implemented in the frame_tracker - //Use the the ActionInterface of the FrameTracker instead of the ServiceCalls and modify constraints in FrameTracker accordingly -// if(failure_counter > 100) -// { -// ROS_ERROR("Endeffector failed to track target_frame!"); -// success = false; -// break; -// } ros::spinOnce(); rate.sleep(); } @@ -328,7 +302,7 @@ void CartesianController::goalCallback() return; } - movePTP(action_struct.move_lin.end, 0.001); +// movePTP(action_struct.move_lin.end, 0.001); // De-Activate Tracking if(!stopTracking()) @@ -342,45 +316,37 @@ void CartesianController::goalCallback() else if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::CIRC) { ROS_INFO("move_circ"); -// if(!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct)) -// { -// actionAbort(false, "Failed to do interpolation for 'move_circ'"); -// return; -// } -// -// // Publish Preview -// utils_.previewPath(cartesian_path); -// -// // Activate Tracking -// if(!startTracking()) -// { -// actionAbort(false, "Failed to start tracking"); -// return; -// } -// -// // Move to start -// if(!movePTP(cartesian_path.poses.at(0), 0.03)) -// { -// actionAbort(false, "Failed to movePTP to start"); -// return; -// } -// -// // Execute path -// if(!posePathBroadcaster(cartesian_path)) -// { -// actionAbort(false, "Failed to execute path for 'move_circ'"); -// return; -// } -// -// // De-Activate Tracking -// if(!stopTracking()) -// { -// actionAbort(false, "Failed to stop tracking"); -// return; -// } -// -// actionSuccess(true, "move_circ succeeded!"); - actionAbort(false, "Not implemented."); + if(!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct)) + { + actionAbort(false, "Failed to do interpolation for 'move_circ'"); + return; + } + + // Publish Preview + utils_.previewPath(cartesian_path); + + // Activate Tracking + if(!startTracking()) + { + actionAbort(false, "Failed to start tracking"); + return; + } + + // Execute path + if(!posePathBroadcaster(cartesian_path)) + { + actionAbort(false, "Failed to execute path for 'move_circ'"); + return; + } + + // De-Activate Tracking + if(!stopTracking()) + { + actionAbort(false, "Failed to stop tracking"); + return; + } + + actionSuccess(true, "move_circ succeeded!"); } else { diff --git a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py index 528986a1..990097a4 100644 --- a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py +++ b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py @@ -74,4 +74,6 @@ def gen_pose(pos=[0,0,0], rpy=[0,0,0]): pose = Pose() pose.position.x, pose.position.y, pose.position.z = pos pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = quaternion_from_euler(*rpy) + + return pose diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 14d779da..4dedf5c8 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -54,10 +54,6 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ double Se_rot = q_start.angleShortestPath(q_end); - ROS_INFO_STREAM("Se_lin: " << Se_lin << " Se_rot: " << Se_rot); - // Calculate path for each Angle -// (std::vector path_matrix[2],const double Se_lin, const double Se_rot,geometry_msgs::Pose start) - if(!trajectory_profile_generator_->calculateProfile(path_matrix, Se_lin, Se_rot, as.move_lin.start)) { return false; @@ -71,7 +67,7 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ norm_factor = 1/angular_path.back(); path = angular_path; } - else + else //todo: There's still a bug in the rotational interpolation ! { norm_factor = 1/linear_path.back(); path = linear_path; @@ -102,61 +98,69 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pose_array, const cob_cartesian_controller::CartesianActionStruct& as) { - this->trajectory_profile_generator_.reset(TrajectoryProfileBuilder::createProfile(as)); - pose_array.header.stamp = ros::Time::now(); - pose_array.header.frame_id = root_frame_; - - tf::Quaternion q_start, q_end; - tf::Quaternion q; - tf::Transform C, P, T; - - double norm_factor; - tf::quaternionMsgToTF(as.move_lin.start.orientation,q_start); - tf::quaternionMsgToTF(as.move_lin.end.orientation,q_end); - - std::vector linear_path, angular_path, path; - - std::vector path_array; - std::vector path_matrix[2]; - - geometry_msgs::Pose pose; - - double Se_rot = as.move_circ.end_angle - as.move_circ.start_angle; + pose_array.header.frame_id = root_frame_; + + tf::Quaternion q; + tf::Transform C, P, T; + + std::vector path_array; + std::vector path_matrix[4]; + + geometry_msgs::Pose pose; + + double Se = as.move_circ.end_angle - as.move_circ.start_angle; + + bool forward; + if(Se < 0) + { + forward = false; + } + else + { + forward = true; + } + Se = std::fabs(Se); + + // Calculates the Path with RAMP or SINOID profile + if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0, pose)) + { + return false; + } + + path_array = path_matrix[0]; + // Define Center Pose + C.setOrigin(tf::Vector3(as.move_circ.pose_center.position.x, as.move_circ.pose_center.position.y, as.move_circ.pose_center.position.z)); + tf::quaternionMsgToTF(as.move_circ.pose_center.orientation, q); + C.setRotation(q); + + // Interpolate the circular path + for(int i = 0 ; i < path_array.size() ; i++) + { + // Rotate T + T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); + + if(forward) + { + T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); + q.setRPY(0, -path_array.at(i), 0); + } + else + { + T.setOrigin(tf::Vector3(cos(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius, 0, sin(as.move_circ.start_angle - path_array.at(i)) * as.move_circ.radius)); + q.setRPY(0, path_array.at(i), 0); + } + + T.setRotation(q); + + // Calculate TCP Position + P = C * T; + + tf::pointTFToMsg(P.getOrigin(), pose.position); + tf::quaternionTFToMsg(P.getRotation(), pose.orientation); + + pose_array.poses.push_back(pose); + } - Se_rot = std::fabs(Se_rot); - - // Calculates the Path with RAMP or SINOID profile - if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se_rot, 0, pose)) - { - return false; - } - - linear_path = path_matrix[0]; - angular_path = path_matrix[1]; - - // Define Center Pose - C.setOrigin(tf::Vector3(as.move_circ.pose_center.position.x, as.move_circ.pose_center.position.y, as.move_circ.pose_center.position.z)); - tf::quaternionMsgToTF(as.move_circ.pose_center.orientation, q); - C.setRotation(q); - - // Interpolate the circular path - for(int i = 0 ; i < linear_path.size() ; i++) - { - ROS_INFO_STREAM("linear_path.at(" << i << "): " << linear_path.at(i)); - - // Rotate T - T.setOrigin(tf::Vector3(cos(linear_path.at(i)) * as.move_circ.radius, 0, sin(linear_path.at(i)) * as.move_circ.radius)); - q.setRPY(0, path_array.at(i), 0); - T.setRotation(q); - - // Calculate TCP Position - P = C * T; - - tf::pointTFToMsg(P.getOrigin(), pose.position); - tf::quaternionTFToMsg(P.getRotation(), pose.orientation); - - pose_array.poses.push_back(pose); - } return true; } diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index 47cdef12..7e1e0ec2 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -60,13 +60,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo; pt.vel = vel; - - ROS_INFO_STREAM("========================================================================="); - ROS_INFO_STREAM("vel_new:" << vel << " accl: " << accl << " Se: " << std::fabs(Se)); - ROS_INFO_STREAM("pt.tb:" << pt.tb << " pt.tv: " << pt.tv << " pt.te: " << pt.te); - ROS_INFO_STREAM("pt.steps_tb:" << pt.steps_tb << " pt.steps_tv: " << pt.steps_tv << " pt.steps_te: " << pt.steps_te); - ROS_INFO_STREAM("========================================================================="); - pt.ok = true; } else diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index 7d92a459..0a0db28a 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -61,13 +61,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo; pt.vel = vel; - - ROS_INFO_STREAM("========================================================================="); - ROS_INFO_STREAM("vel_new:" << vel << " accl: " << accl << " Se: " << std::fabs(Se)); - ROS_INFO_STREAM("pt.tb:" << pt.tb << " pt.tv: " << pt.tv << " pt.te: " << pt.te); - ROS_INFO_STREAM("pt.steps_tb:" << pt.steps_tb << " pt.steps_tv: " << pt.steps_tv << " pt.steps_te: " << pt.steps_te); - ROS_INFO_STREAM("========================================================================="); - pt.ok = true; } else From fb12ad3e0c46a18b1f86f7b3358e311738525a14 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 8 Dec 2015 17:44:32 +0100 Subject: [PATCH 15/35] Added waitForTransform in the getTransform function.. It threw an transform exception --- cob_frame_tracker/src/cob_frame_tracker.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/cob_frame_tracker/src/cob_frame_tracker.cpp b/cob_frame_tracker/src/cob_frame_tracker.cpp index 803ded75..033eca0b 100644 --- a/cob_frame_tracker/src/cob_frame_tracker.cpp +++ b/cob_frame_tracker/src/cob_frame_tracker.cpp @@ -224,6 +224,7 @@ bool CobFrameTracker::getTransform(const std::string& from, const std::string& t try { + tf_listener_.waitForTransform(from, to, ros::Time(0), ros::Duration(0.2)); tf_listener_.lookupTransform(from, to, ros::Time(0), stamped_tf); transform = true; } From 7b493fe145e2346a6756939a2d8077b4c2198093 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 8 Dec 2015 18:10:28 +0100 Subject: [PATCH 16/35] Almost finalized version --- .../cartesian_controller.h | 2 +- .../cartesian_controller_data_types.h | 4 - .../cartesian_controller_utils.h | 4 +- .../trajectory_interpolator.h | 3 +- .../trajectory_profile_generator_base.h | 24 +++--- .../trajectory_profile_generator_builder.h | 4 +- .../trajectory_profile_generator_ramp.h | 6 +- .../trajectory_profile_generator_sinoid.h | 4 +- cob_cartesian_controller/msg/MoveCirc.msg | 4 +- cob_cartesian_controller/msg/MoveLin.msg | 4 +- .../scripts/lwa4d/lwa4d_circle.py | 84 +++++++++++++++++-- .../scripts/lwa4d/lwa4d_quad.py | 26 +++--- .../src/cartesian_controller.cpp | 78 +++++++---------- .../src/cartesian_controller_utils.cpp | 21 ++--- .../simple_cartesian_interface.py | 2 - .../trajectory_interpolator.cpp | 27 +++--- .../trajectory_profile_generator_builder.cpp | 25 ++++-- .../trajectory_profile_generator_ramp.cpp | 11 +-- .../trajectory_profile_generator_sinoid.cpp | 8 +- 19 files changed, 188 insertions(+), 153 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h index 2ccd9dd7..992fcb02 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h @@ -19,7 +19,7 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: July, 2015 + * \date Date of creation: December, 2015 * * \brief * ... diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 01e7cdc7..f599eb52 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -61,7 +61,6 @@ namespace cob_cartesian_controller geometry_msgs::Pose pose_center; double start_angle, end_angle; double radius; - bool rotate_only; }; struct CartesianActionStruct @@ -72,8 +71,6 @@ namespace cob_cartesian_controller ProfileStruct profile; }; - - class PathArray{ public: PathArray(double Se, std::vector array): @@ -101,7 +98,6 @@ namespace cob_cartesian_controller { pm_.push_back(pa1); pm_.push_back(pa2); - } ~PathMatrix() { diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h index 0b9863f1..edb5e1a0 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h @@ -19,8 +19,8 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: July, 2015 - * + * \date Date of creation: December, 2015 + * This class contains useful functions which are not directly part of the cartesian controller. * \brief * ... * diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h index 54affb65..467def92 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h @@ -19,7 +19,7 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: July, 2015 + * \date Date of creation: December, 2015 * * \brief * ... @@ -37,7 +37,6 @@ #include - class TrajectoryInterpolator { public: diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 3cb47cdc..c4ea0b02 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -14,32 +14,33 @@ * \note * ROS stack name: cob_control * \note - * ROS package name: cob_twist_controller + * ROS package name: cob_cartesian_controller * * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: June, 2015 - * + * \date Date of creation: December, 2015 + * Base class implementation for trajectory_profile_generator. * \brief - * This header contains the interface description of all available - * hardware interface types (position/velocity). + * * ****************************************************************/ -#ifndef HARDWARE_INTERFACE_TYPE_BASE_H_ -#define HARDWARE_INTERFACE_TYPE_BASE_H_ + +#ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H_ +#define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H_ + +#define MIN_VELOCITY_THRESHOLD 0.001 #include "ros/ros.h" #include "cob_cartesian_controller/cartesian_controller_data_types.h" #include -/// Base class for hardware interfaces types. class TrajectoryProfileBase { public: TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct& params): params_(params) - {vel_max_=0;} + {} virtual ~TrajectoryProfileBase() {} @@ -53,12 +54,9 @@ class TrajectoryProfileBase double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; private: virtual bool generatePath(cob_cartesian_controller::PathArray &pa) = 0; -// virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double te_max, double accl, double vel) = 0; - protected: const cob_cartesian_controller::CartesianActionStruct ¶ms_; - double vel_max_; }; -#endif /* HARDWARE_INTERFACE_TYPE_BASE_H_ */ +#endif diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h index 9f7b8d33..d696c224 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h @@ -19,8 +19,8 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: September, 2015 - * + * \date Date of creation: December, 2015 + * Builder class for generic profile generator. * \brief * ****************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index 90491ed4..4d5f352f 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -19,7 +19,7 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: September, 2015 + * \date Date of creation: December, 2015 * * \brief * @@ -27,12 +27,10 @@ #ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ #define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ - #include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h" /* BEGIN TrajectoryProfileRamp ****************************************************************************************/ -/// Class providing a HardwareInterface publishing velocities. -class TrajectoryProfileRamp: public TrajectoryProfileBase +class TrajectoryProfileRamp: public TrajectoryProfileBase { public: TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index f48062dc..175e943d 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -19,7 +19,7 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: September, 2015 + * \date Date of creation: December, 2015 * * \brief * @@ -30,7 +30,6 @@ #include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h" /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ -/// Class providing a HardwareInterface publishing positions. class TrajectoryProfileSinoid : public TrajectoryProfileBase { public: @@ -48,7 +47,6 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); private: -// virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel); cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileSinoid **********************************************************************************************/ diff --git a/cob_cartesian_controller/msg/MoveCirc.msg b/cob_cartesian_controller/msg/MoveCirc.msg index 1c4f2cda..8fb423ff 100644 --- a/cob_cartesian_controller/msg/MoveCirc.msg +++ b/cob_cartesian_controller/msg/MoveCirc.msg @@ -3,6 +3,4 @@ string frame_id float64 start_angle float64 end_angle -float64 radius - -bool rotate_only +float64 radius \ No newline at end of file diff --git a/cob_cartesian_controller/msg/MoveLin.msg b/cob_cartesian_controller/msg/MoveLin.msg index 1f626d5e..fcfb893c 100644 --- a/cob_cartesian_controller/msg/MoveLin.msg +++ b/cob_cartesian_controller/msg/MoveLin.msg @@ -1,4 +1,2 @@ geometry_msgs/Pose pose_goal -string frame_id - -bool rotate_only +string frame_id \ No newline at end of file diff --git a/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py b/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py index 6a23270b..e8fbb9af 100755 --- a/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py +++ b/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py @@ -19,7 +19,7 @@ def init_pos(): pose = sci.gen_pose(pos=[-0.2, 0, 0.8], rpy=[0, 0.0, 0]) profile = Profile() - profile.vel = 0.1 + profile.vel = 0.2 profile.accl = 0.1 profile.profile_type = Profile.SINOID #profile.profile_type = Profile.RAMP @@ -37,8 +37,8 @@ def init_pos(): start_angle = 180.0 * math.pi / 180.0 end_angle = 0.0 * math.pi / 180.0 profile = Profile() - profile.vel = 0.1 - profile.accl = 0.1 + profile.vel = 0.4 + profile.accl = 0.3 profile.profile_type = Profile.SINOID #profile.profile_type = Profile.RAMP @@ -49,10 +49,26 @@ def init_pos(): rospy.logerr(message) - pose = sci.gen_pose(pos=[0, 0, 1], rpy=[0, 0.0, -math.pi/2]) + pose = sci.gen_pose(pos=[0.2, 0.0, 0.8], rpy=[0, math.pi/2, 0]) profile = Profile() - profile.vel = 0.1 - profile.accl = 0.1 + profile.vel = 0.3 + profile.accl = 0.3 + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + + + pose = sci.gen_pose(pos=[0.0, -0.2, 0.8], rpy=[0, 0.0, math.pi/2]) + profile = Profile() + profile.vel = 0.3 + profile.accl = 0.3 profile.profile_type = Profile.SINOID #profile.profile_type = Profile.RAMP @@ -63,4 +79,58 @@ def init_pos(): rospy.logerr(message) #init_pos() - rospy.sleep(3.0) + + + + + pose = sci.gen_pose(pos=[0.0, 0.0, 0.8], rpy=[0.0, 0.0, math.pi/2]) + start_angle = 180.0 * math.pi / 180.0 + end_angle = 0.0 * math.pi / 180.0 + profile = Profile() + profile.vel = 0.4 + profile.accl = 0.3 + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP + + success, message = sci.move_circ(pose, "world", start_angle, end_angle, 0.2, profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + + + + pose = sci.gen_pose(pos=[0.0, 0.2, 0.8], rpy=[0, math.pi/2, 0]) + profile = Profile() + profile.vel = 0.3 + profile.accl = 0.3 + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + #init_pos() + + + + pose = sci.gen_pose(pos=[-0.2, 0.0, 0.8], rpy=[0, 0, 0]) + profile = Profile() + profile.vel = 0.3 + profile.accl = 0.3 + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP + + success, message = sci.move_lin(pose, "world", profile) + if success: + rospy.loginfo(message) + else: + rospy.logerr(message) + + init_pos() + + diff --git a/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py b/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py index 24d896e7..027d9dcb 100755 --- a/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py +++ b/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py @@ -34,12 +34,12 @@ def init_pos(): rospy.sleep(3.0) - pose = sci.gen_pose(pos=[0.3, 0, 0.9], rpy=[0.0, 0.0, 90.0]) + pose = sci.gen_pose(pos=[0.3, 0, 0.9], rpy=[0.0, 0.0, math.pi/3]) profile = Profile() profile.vel = 0.1 profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: @@ -55,8 +55,8 @@ def init_pos(): profile = Profile() profile.vel = 0.1 profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: @@ -73,8 +73,8 @@ def init_pos(): profile = Profile() profile.vel = 0.1 profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: @@ -90,8 +90,8 @@ def init_pos(): profile = Profile() profile.vel = 0.1 profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: @@ -108,8 +108,8 @@ def init_pos(): profile = Profile() profile.vel = 0.1 profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: @@ -126,8 +126,8 @@ def init_pos(): profile = Profile() profile.vel = 0.1 profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP + profile.profile_type = Profile.SINOID + #profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index 0b07d104..2b427423 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -19,8 +19,9 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: July, 2015 - * + * \date Date of creation: December, 2015 + * This class is used to interpolate cartesian paths with a velocity profile. There are two types of interpolations (Linear and Circular) and two velocity + * profiles (Ramp and Sinoid) implemented. * \brief * ... * @@ -202,6 +203,7 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca int failure_counter = 0; ros::Rate rate(update_rate_); tf::Transform transform; + for(unsigned int i = 0; i < cartesian_path.poses.size(); i++) { if(!as_->isActive()) @@ -221,39 +223,35 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); - // Get transformation - tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); - ros::spinOnce(); rate.sleep(); } - while(true) - { - // Get transformation - tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); - ROS_INFO_STREAM("stamped: x: " << stamped_transform.getOrigin().getX() << " y: " << stamped_transform.getOrigin().getY() << " z: " << stamped_transform.getOrigin().getZ()); - - if(!utils_.inEpsilonArea(stamped_transform, 0.0005)) - { - // Send/Refresh target Frame - transform.setOrigin( tf::Vector3(cartesian_path.poses.back().position.x, - cartesian_path.poses.back().position.y, - cartesian_path.poses.back().position.z) ); - transform.setRotation( tf::Quaternion(cartesian_path.poses.back().orientation.x, - cartesian_path.poses.back().orientation.y, - cartesian_path.poses.back().orientation.z, - cartesian_path.poses.back().orientation.w) ); - tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); - } - else - { - break; - } - - ros::spinOnce(); - rate.sleep(); - } +// while(ros::ok()) +// { +// // Get transformation +// tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); +// ROS_INFO_STREAM("Adjustment (This compensates the inaccuracy from the T_IPO interpolation). This is optional and results in an velocity spike !!! \n : x: " << stamped_transform.getOrigin().getX() << " y: " << stamped_transform.getOrigin().getY() << " z: " << stamped_transform.getOrigin().getZ()); +// if(!utils_.inEpsilonArea(stamped_transform, 0.0005)) +// { +// // Send/Refresh target Frame +// transform.setOrigin( tf::Vector3(cartesian_path.poses.back().position.x, +// cartesian_path.poses.back().position.y, +// cartesian_path.poses.back().position.z) ); +// transform.setRotation( tf::Quaternion(cartesian_path.poses.back().orientation.x, +// cartesian_path.poses.back().orientation.y, +// cartesian_path.poses.back().orientation.z, +// cartesian_path.poses.back().orientation.w) ); +// tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); +// } +// else +// { +// break; +// } +// +// ros::spinOnce(); +// rate.sleep(); +// } return success; } @@ -261,23 +259,13 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca void CartesianController::goalCallback() { - geometry_msgs::Pose start_pose, end_pose, pose; geometry_msgs::PoseArray cartesian_path; cob_cartesian_controller::CartesianActionStruct action_struct; - - ROS_INFO_STREAM("========================================================================="); - ROS_INFO("Received a new goal"); - ROS_INFO_STREAM("========================================================================="); - - start_pose = utils_.getPose(root_frame_, chain_tip_link_); - action_struct = acceptGoal(as_->acceptNewGoal()); if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::LIN) { - ROS_INFO("move_lin"); - // Interpolate path if(!trajectory_interpolator_->linearInterpolation(cartesian_path, action_struct)) { @@ -302,8 +290,6 @@ void CartesianController::goalCallback() return; } -// movePTP(action_struct.move_lin.end, 0.001); - // De-Activate Tracking if(!stopTracking()) { @@ -315,7 +301,6 @@ void CartesianController::goalCallback() } else if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::CIRC) { - ROS_INFO("move_circ"); if(!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct)) { actionAbort(false, "Failed to do interpolation for 'move_circ'"); @@ -366,10 +351,6 @@ cob_cartesian_controller::MoveLinStruct CartesianController::convertMoveLin(cons move_lin.start = start; move_lin.end = end; - ROS_WARN("move_lin_msg_orientation .. w: %f", move_lin_msg.pose_goal.orientation.w); - ROS_WARN("move_lin_msg_orientation .. x: %f", move_lin_msg.pose_goal.orientation.x); - ROS_WARN("move_lin_msg_orientation .. y: %f", move_lin_msg.pose_goal.orientation.y); - ROS_WARN("move_lin_msg_orientation .. z: %f", move_lin_msg.pose_goal.orientation.z); return move_lin; } @@ -383,7 +364,6 @@ cob_cartesian_controller::MoveCircStruct CartesianController::convertMoveCirc(co move_circ.end_angle = move_circ_msg.end_angle; move_circ.radius = move_circ_msg.radius; - move_circ.pose_center = center; return move_circ; diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index 0ec3eeed..6fafe4b2 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -19,8 +19,8 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: July, 2015 - * + * \date Date of creation: December, 2015 + * This class contains useful functions which are not directly part of the cartesian controller. * \brief * ... * @@ -78,7 +78,7 @@ void CartesianControllerUtils::transformPose(const std::string source_frame, con try { stamped_in.header.stamp = ros::Time(0); - tf_listener_.waitForTransform(target_frame, source_frame, stamped_in.header.stamp, ros::Duration(0.5)); + tf_listener_.waitForTransform(target_frame, source_frame, stamped_in.header.stamp, ros::Duration(1.0)); tf_listener_.transformPose(target_frame, stamped_in, stamped_out); pose_out = stamped_out.pose; transform = true; @@ -95,9 +95,6 @@ void CartesianControllerUtils::transformPose(const std::string source_frame, con /// Used to check whether the chain_tip_link is close to the target_frame /// 'stamped_transform' expreses the transform between the two frames. /// Thus inEpsilonArea() returns 'true' in case 'stamped_transform' is "smaller" than 'epsilon' -// ToDo: Can we simplify this check? -// e.g. use stamped_transform.getOrigin().length() < epsilon -// what about orientation distance? bool CartesianControllerUtils::inEpsilonArea(const tf::StampedTransform& stamped_transform, const double epsilon) { double roll, pitch, yaw; @@ -150,11 +147,11 @@ void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray& pose_ double id = marker_array_.markers.size(); for(unsigned int i=0; i < pose_array.poses.size(); i++) - { - marker.id = id+i; - marker.pose = pose_array.poses.at(i); - marker_array_.markers.push_back(marker); - } + { + marker.id = id+i; + marker.pose = pose_array.poses.at(i); + marker_array_.markers.push_back(marker); + } marker_pub_.publish(marker_array_); } @@ -186,5 +183,5 @@ void CartesianControllerUtils::copyMatrix(std::vector *path_array,std::v double CartesianControllerUtils::roundUpToMultiplier(double numberToRound, double multiplier) { - return ( multiplier - fmod(numberToRound,multiplier) + numberToRound ); + return ( multiplier - std::fmod(numberToRound,multiplier) + numberToRound ); } diff --git a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py index 990097a4..548c01e8 100644 --- a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py +++ b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py @@ -20,7 +20,6 @@ def move_lin(pose_goal, frame_id, profile, rotate_only=False): goal.move_type = CartesianControllerGoal.LIN goal.move_lin.pose_goal = pose_goal goal.move_lin.frame_id = frame_id - goal.move_lin.rotate_only = rotate_only goal.profile = profile # print goal @@ -44,7 +43,6 @@ def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile, ro goal.move_type = CartesianControllerGoal.CIRC goal.move_circ.pose_center = pose_center goal.move_circ.frame_id = frame_id - goal.move_circ.rotate_only = rotate_only goal.move_circ.start_angle = start_angle goal.move_circ.end_angle = end_angle goal.move_circ.radius = radius diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 4dedf5c8..1811ba4a 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -19,8 +19,8 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: July, 2015 - * + * \date Date of creation: December, 2015 + * This class contains the implementation for the linear and circular interpolation. * \brief * ... * @@ -49,8 +49,8 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ tf::quaternionMsgToTF(as.move_lin.end.orientation,q_end); double Se_lin = sqrt(pow((as.move_lin.end.position.x - as.move_lin.start.position.x), 2) + - pow((as.move_lin.end.position.y - as.move_lin.start.position.y), 2) + - pow((as.move_lin.end.position.z - as.move_lin.start.position.z), 2)); + pow((as.move_lin.end.position.y - as.move_lin.start.position.y), 2) + + pow((as.move_lin.end.position.z - as.move_lin.start.position.z), 2)); double Se_rot = q_start.angleShortestPath(q_end); @@ -62,19 +62,18 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ linear_path = path_matrix[0]; angular_path = path_matrix[1]; - if(linear_path.back() == 0) + if(fabs(linear_path.back()) > fabs(angular_path.back())) { - norm_factor = 1/angular_path.back(); - path = angular_path; + path = linear_path; } - else //todo: There's still a bug in the rotational interpolation ! + else { - norm_factor = 1/linear_path.back(); - path = linear_path; + path = angular_path; } + norm_factor = 1/path.back(); // Interpolate the linear path - for(unsigned int i = 0 ; i < path_matrix[0].size() ; i++) + for(unsigned int i = 0 ; i < linear_path.size() ; i++) { if(linear_path.back() == 0) { @@ -98,20 +97,22 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pose_array, const cob_cartesian_controller::CartesianActionStruct& as) { - pose_array.header.stamp = ros::Time::now(); + pose_array.header.stamp = ros::Time::now(); pose_array.header.frame_id = root_frame_; tf::Quaternion q; tf::Transform C, P, T; std::vector path_array; - std::vector path_matrix[4]; + std::vector path_matrix[2]; geometry_msgs::Pose pose; double Se = as.move_circ.end_angle - as.move_circ.start_angle; bool forward; + + // Needed for the circle direction ! if(Se < 0) { forward = false; diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp index ccd5fdc7..e4a6863b 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp @@ -1,4 +1,3 @@ - /*! ***************************************************************** * \file @@ -15,34 +14,40 @@ * \note * ROS stack name: cob_control * \note - * ROS package name: cob_twist_controller + * ROS package name: cob_cartesian_controller * * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: June, 2015 + * \date Date of creation: December, 2015 * * \brief - * This module contains the implementation of all interface types. + * This module contains the implementation of all profile types. * ****************************************************************/ -#define RAMP_PROFILE 1 -#define SINOID_PROFILE 2 + +#ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BUILDER_H_ +#define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BUILDER_H_ + #include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h" #include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" #include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h" +#include "cob_cartesian_controller/Profile.h" /* BEGIN TrajectoryProfileBase *****************************************************************************************/ - TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesian_controller::CartesianActionStruct& params) { + cob_cartesian_controller::Profile msg; + + const int RAMP = static_cast(msg.RAMP); + const int SINOID = static_cast(msg.SINOID); TrajectoryProfileBase* ib = NULL; switch(params.profile.profile_type) { - case RAMP_PROFILE: + case RAMP: ib = new TrajectoryProfileRamp(params); break; - case SINOID_PROFILE: + case SINOID: ib = new TrajectoryProfileSinoid(params); break; default: @@ -53,3 +58,5 @@ TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesi return ib; } /* END TrajectoryProfileBase *******************************************************************************************/ + +#endif diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index 7e1e0ec2..9ab3b9a3 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -19,12 +19,12 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: September, 2015 - * + * \date Date of creation: December, 2015 + * This class calculates the timings for a ramp velocity profile with respect to the interpolation rate of the system. It also adjusts the velocity in case of + * a bad velocity / path-length ratio, which leads to a longer path time. * \brief * ****************************************************************/ - #include "ros/ros.h" #include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" /* BEGIN TrajectoryProfileRamp ********************************************************************************************/ @@ -32,7 +32,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil { CartesianControllerUtils utils; cob_cartesian_controller::ProfileTimings pt; - int steps_te, steps_tv, steps_tb = 0; double tv, tb = 0.0; // Calculate the Sinoid Profile Parameters @@ -41,7 +40,7 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil vel = sqrt(std::fabs(Se) * accl); } - if(vel > 0.00001) + if(vel > MIN_VELOCITY_THRESHOLD) { tb = utils.roundUpToMultiplier(vel / accl, params_.profile.t_ipo); if(calcMaxTe) @@ -129,13 +128,11 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat cob_cartesian_controller::PathArray lin(Se_lin, linear_path); cob_cartesian_controller::PathArray rot(Se_rot, angular_path); - cob_cartesian_controller::PathMatrix pm(lin,rot); // Get the profile timings from the longest path pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); - ROS_INFO_STREAM("pt_max_SEEEEEEEEEEEEE: " << pm.getMaxSe()); // Calculate the paths for(int i=0; i < pm.pm_.size(); i++) { diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index 0a0db28a..f29aa9c5 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -19,7 +19,9 @@ * \author * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * - * \date Date of creation: September, 2015 + * \date Date of creation: December, 2015 + * This class calculates the timings for a sinoid velocity profile with respect to the interpolation rate of the system. It also adjusts the velocity in case of + * a bad velocity / path-length ratio, which leads to a longer path time. * * \brief * @@ -32,7 +34,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf { CartesianControllerUtils utils; cob_cartesian_controller::ProfileTimings pt; - int steps_te, steps_tv, steps_tb = 0; double tv, tb = 0.0; // Calculate the Sinoid Profile Parameters @@ -41,7 +42,7 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf vel = sqrt(std::fabs(Se) * accl / 2); } - if(vel > 0.001) + if(vel > MIN_VELOCITY_THRESHOLD) { tb = utils.roundUpToMultiplier(2 * vel / accl, params_.profile.t_ipo); @@ -130,7 +131,6 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m cob_cartesian_controller::PathArray lin(Se_lin, linear_path); cob_cartesian_controller::PathArray rot(Se_rot, angular_path); - cob_cartesian_controller::PathMatrix pm(lin,rot); // Get the profile timings from the longest path From 1903fb0aae0356472d31b57cea45d13ff96bace8 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 8 Dec 2015 18:14:23 +0100 Subject: [PATCH 17/35] Final version --- .../src/cartesian_controller.cpp | 26 ------------------- 1 file changed, 26 deletions(-) diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index 2b427423..5b61084e 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -227,32 +227,6 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca rate.sleep(); } -// while(ros::ok()) -// { -// // Get transformation -// tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); -// ROS_INFO_STREAM("Adjustment (This compensates the inaccuracy from the T_IPO interpolation). This is optional and results in an velocity spike !!! \n : x: " << stamped_transform.getOrigin().getX() << " y: " << stamped_transform.getOrigin().getY() << " z: " << stamped_transform.getOrigin().getZ()); -// if(!utils_.inEpsilonArea(stamped_transform, 0.0005)) -// { -// // Send/Refresh target Frame -// transform.setOrigin( tf::Vector3(cartesian_path.poses.back().position.x, -// cartesian_path.poses.back().position.y, -// cartesian_path.poses.back().position.z) ); -// transform.setRotation( tf::Quaternion(cartesian_path.poses.back().orientation.x, -// cartesian_path.poses.back().orientation.y, -// cartesian_path.poses.back().orientation.z, -// cartesian_path.poses.back().orientation.w) ); -// tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); -// } -// else -// { -// break; -// } -// -// ros::spinOnce(); -// rate.sleep(); -// } - return success; } From fd054f5c9b0a9c0baa567dfd03fb71fb63287371 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 13:10:20 +0100 Subject: [PATCH 18/35] adjust description --- cob_cartesian_controller/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cob_cartesian_controller/package.xml b/cob_cartesian_controller/package.xml index d4d90349..a123f61c 100644 --- a/cob_cartesian_controller/package.xml +++ b/cob_cartesian_controller/package.xml @@ -2,8 +2,8 @@ cob_cartesian_controller 0.6.10 - This package provides nodes that broadcast tf-frames along various (model-based) Cartesian paths. - The tf-frames can be used as targets for the cob_frame_tracker/cob_twist_controller + This package provides nodes that broadcast tf-frames along various (model-based) Cartesian paths (e.g. Linear, Circular). + The tf-frames are interpolated using a given velocity profile (e.g. Ramp, Sinoid) and can be used as targets for the cob_frame_tracker/cob_twist_controller. Christoph Mark From 1682bc603e0910c0cb6e467092ff61d0c4d35b03 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 13:10:50 +0100 Subject: [PATCH 19/35] adjust descriptions ins license plate --- .../cob_cartesian_controller/cartesian_controller.h | 4 +++- .../cartesian_controller_data_types.h | 2 +- .../cob_cartesian_controller/cartesian_controller_utils.h | 4 ++-- .../trajectory_interpolator/trajectory_interpolator.h | 2 +- .../trajectory_profile_generator_base.h | 4 ++-- .../trajectory_profile_generator_builder.h | 3 ++- .../trajectory_profile_generator_ramp.h | 1 + .../trajectory_profile_generator_sinoid.h | 1 + cob_cartesian_controller/src/cartesian_controller.cpp | 7 ++++--- .../src/cartesian_controller_utils.cpp | 4 ++-- .../trajectory_interpolator/trajectory_interpolator.cpp | 4 ++-- .../trajectory_profile_generator_ramp.cpp | 4 ++-- .../trajectory_profile_generator_sinoid.cpp | 3 +-- 13 files changed, 24 insertions(+), 19 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h index 992fcb02..74ba129f 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h @@ -22,7 +22,9 @@ * \date Date of creation: December, 2015 * * \brief - * ... + * This class is used to interpolate various Cartesian paths for a given velocity profile. + * Supported types of Cartesian paths are Linear and Circular. + * Supported types of velocity profiles are Ramp and Sinoid. * ****************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index f599eb52..6247f3b6 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -22,7 +22,7 @@ * \date Date of creation: July, 2015 * * \brief - * ... + * Definition of data structures used in the cob_cartesian_controller package. * ****************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h index edb5e1a0..25a3482a 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h @@ -20,9 +20,9 @@ * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * * \date Date of creation: December, 2015 - * This class contains useful functions which are not directly part of the cartesian controller. + * * \brief - * ... + * Helper functions used in the cob_cartesian_controller package. * ****************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h index 467def92..66b21112 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h @@ -22,7 +22,7 @@ * \date Date of creation: December, 2015 * * \brief - * ... + * This class contains the implementation for the linear and circular interpolation. * ****************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index c4ea0b02..7b8165e9 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -20,9 +20,9 @@ * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * * \date Date of creation: December, 2015 - * Base class implementation for trajectory_profile_generator. - * \brief * + * \brief + * Base class for trajectory_profile_generator. * ****************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h index d696c224..eedc1be8 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h @@ -20,8 +20,9 @@ * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * * \date Date of creation: December, 2015 - * Builder class for generic profile generator. + * * \brief + * Builder class for generic profile generator. * ****************************************************************/ #ifndef TRAJECTORY_PROFILE_BUILDER_H_ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index 4d5f352f..6bd6c229 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -22,6 +22,7 @@ * \date Date of creation: December, 2015 * * \brief + * Class implementing the Ramp velocity profile generator. * ****************************************************************/ #ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 175e943d..45c6ba05 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -22,6 +22,7 @@ * \date Date of creation: December, 2015 * * \brief + * Class implementing the Sinoid velocity profile generator. * ****************************************************************/ #ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index 5b61084e..b17a4a9b 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -20,10 +20,11 @@ * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * * \date Date of creation: December, 2015 - * This class is used to interpolate cartesian paths with a velocity profile. There are two types of interpolations (Linear and Circular) and two velocity - * profiles (Ramp and Sinoid) implemented. + * * \brief - * ... + * This class is used to interpolate various Cartesian paths for a given velocity profile. + * Supported types of Cartesian paths are Linear and Circular. + * Supported types of velocity profiles are Ramp and Sinoid. * ****************************************************************/ #include diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index 6fafe4b2..37f945a2 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -20,9 +20,9 @@ * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * * \date Date of creation: December, 2015 - * This class contains useful functions which are not directly part of the cartesian controller. + * * \brief - * ... + * Helper functions used in the cob_cartesian_controller package. * ****************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 1811ba4a..737d2b8e 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -20,9 +20,9 @@ * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * * \date Date of creation: December, 2015 - * This class contains the implementation for the linear and circular interpolation. + * * \brief - * ... + * This class contains the implementation for the linear and circular interpolation. * ****************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index 9ab3b9a3..f6257417 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -20,9 +20,9 @@ * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * * \date Date of creation: December, 2015 - * This class calculates the timings for a ramp velocity profile with respect to the interpolation rate of the system. It also adjusts the velocity in case of - * a bad velocity / path-length ratio, which leads to a longer path time. + * * \brief + * Class implementing the Ramp velocity profile generator. * ****************************************************************/ #include "ros/ros.h" diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index f29aa9c5..a89dfd2d 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -20,10 +20,9 @@ * Author: Christoph Mark, email: christoph.mark@ipa.fraunhofer.de / christoph.mark@gmail.com * * \date Date of creation: December, 2015 - * This class calculates the timings for a sinoid velocity profile with respect to the interpolation rate of the system. It also adjusts the velocity in case of - * a bad velocity / path-length ratio, which leads to a longer path time. * * \brief + * Class implementing the Sinoid velocity profile generator. * ****************************************************************/ #include "ros/ros.h" From cb7d57b0d8b4f1472973d7ba21de530e77bdbecf Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 13:57:56 +0100 Subject: [PATCH 20/35] towards code styling consistency --- .../cartesian_controller.h | 5 +- .../cartesian_controller_data_types.h | 67 ++++++++++--------- .../cartesian_controller_utils.h | 18 +++-- .../trajectory_interpolator.h | 13 ++-- .../trajectory_profile_generator_base.h | 46 +++++++------ .../trajectory_profile_generator_builder.h | 20 +++--- .../trajectory_profile_generator_ramp.h | 39 ++++++----- .../trajectory_profile_generator_sinoid.h | 39 ++++++----- .../src/cartesian_controller.cpp | 1 + .../src/cartesian_controller_node.cpp | 16 ++--- .../src/cartesian_controller_utils.cpp | 21 +++--- .../trajectory_interpolator.cpp | 9 +-- .../trajectory_profile_generator_builder.cpp | 17 ++--- .../trajectory_profile_generator_ramp.cpp | 18 ++--- .../trajectory_profile_generator_sinoid.cpp | 4 +- 15 files changed, 167 insertions(+), 166 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h index 74ba129f..996b85df 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h @@ -31,11 +31,11 @@ #ifndef CARTESIAN_CONTROLLER_H #define CARTESIAN_CONTROLLER_H -#include #include #include #include +#include #include #include #include @@ -98,7 +98,6 @@ class CartesianController CartesianControllerUtils utils_; boost::shared_ptr< TrajectoryInterpolator > trajectory_interpolator_; - }; -#endif +#endif // CARTESIAN_CONTROLLER_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 6247f3b6..8d2ce26d 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -26,12 +26,12 @@ * ****************************************************************/ -#ifndef COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_ -#define COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_ +#ifndef CARTESIAN_CONTROLLER_DATA_STRUCTURES_H +#define CARTESIAN_CONTROLLER_DATA_STRUCTURES_H +#include #include #include -#include namespace cob_cartesian_controller { @@ -71,7 +71,8 @@ namespace cob_cartesian_controller ProfileStruct profile; }; - class PathArray{ + class PathArray + { public: PathArray(double Se, std::vector array): Se_(Se), @@ -88,38 +89,40 @@ namespace cob_cartesian_controller bool calcTe_; double Se_; std::vector array_; - }; - class PathMatrix{ - public: - PathMatrix(PathArray &pa1, - PathArray &pa2) - { - pm_.push_back(pa1); - pm_.push_back(pa2); - } - ~PathMatrix() + class PathMatrix + { + public: + PathMatrix(PathArray& pa1, + PathArray& pa2) + { + pm_.push_back(pa1); + pm_.push_back(pa2); + } + + ~PathMatrix() + { + pm_.clear(); + } + + double getMaxSe() + { + double se_max = 0; + + for (unsigned int i = 0; i < pm_.size(); i++) { - pm_.clear(); + if (se_max < fabs(pm_[i].Se_)) + { + se_max = fabs(pm_[i].Se_); + } } + return se_max; + } - double getMaxSe(); - std::vector pm_; - }; + std::vector pm_; + }; - inline double PathMatrix::getMaxSe() - { - double se_max = 0; +}// namespace - for(int i=0; i #include @@ -35,16 +35,14 @@ #include #include -#include "cob_cartesian_controller/cartesian_controller_data_types.h" - - +#include class CartesianControllerUtils { public: CartesianControllerUtils() { - marker_pub_ = nh_.advertise("cartesian_controller/preview_path",1); + marker_pub_ = nh_.advertise("cartesian_controller/preview_path", 1); } void transformPose(const std::string source_frame, const std::string target_frame, const geometry_msgs::Pose pose_in, geometry_msgs::Pose& pose_out); @@ -56,9 +54,9 @@ class CartesianControllerUtils void previewPath(const geometry_msgs::PoseArray& pose_array); - void sortMatrixByIdx(std::vector &m); - void adjustArrayLength(std::vector &m); - void copyMatrix(std::vector *path_array,std::vector &m); + void sortMatrixByIdx(std::vector& m); + void adjustArrayLength(std::vector& m); + void copyMatrix(std::vector* path_array, std::vector& m); double roundUpToMultiplier(double numberToRound, double multiplier); private: @@ -69,4 +67,4 @@ class CartesianControllerUtils ros::Publisher marker_pub_; }; -#endif /* COB_CARTESIAN_CONTROLLER_UTILS_H_ */ +#endif // CARTESIAN_CONTROLLER_UTILS_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h index 66b21112..4d61c2d2 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h @@ -26,8 +26,8 @@ * ****************************************************************/ -#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H_ -#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H_ +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H #include #include @@ -36,16 +36,15 @@ #include #include - class TrajectoryInterpolator { public: TrajectoryInterpolator(std::string root_frame, double update_rate) : root_frame_(root_frame) - {} - ~TrajectoryInterpolator(){ + ~TrajectoryInterpolator() + { trajectory_profile_generator_.reset(); } @@ -57,8 +56,8 @@ class TrajectoryInterpolator private: std::string root_frame_; - boost::shared_ptr< TrajectoryProfileBase > trajectory_profile_generator_; + boost::shared_ptr trajectory_profile_generator_; }; -#endif /* COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H_ */ +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 7b8165e9..922a9aa3 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -26,37 +26,39 @@ * ****************************************************************/ -#ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H_ -#define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H_ +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H -#define MIN_VELOCITY_THRESHOLD 0.001 - -#include "ros/ros.h" -#include "cob_cartesian_controller/cartesian_controller_data_types.h" +#include +#include #include + +#define MIN_VELOCITY_THRESHOLD 0.001 + class TrajectoryProfileBase { - public: - TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct& params): - params_(params) - {} +public: + TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct& params) + : params_(params) + {} - virtual ~TrajectoryProfileBase() {} + virtual ~TrajectoryProfileBase() + {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) = 0; + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) = 0; - virtual bool calculateProfile(std::vector* path_matrix, - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start) = 0; + virtual bool calculateProfile(std::vector* path_matrix, + const double Se_lin, const double Se_rot, + geometry_msgs::Pose start) = 0; - virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, - double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; - private: - virtual bool generatePath(cob_cartesian_controller::PathArray &pa) = 0; + virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, + double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; +private: + virtual bool generatePath(cob_cartesian_controller::PathArray& pa) = 0; - protected: - const cob_cartesian_controller::CartesianActionStruct ¶ms_; +protected: + const cob_cartesian_controller::CartesianActionStruct& params_; }; -#endif +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h index eedc1be8..5496cd41 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h @@ -25,22 +25,22 @@ * Builder class for generic profile generator. * ****************************************************************/ -#ifndef TRAJECTORY_PROFILE_BUILDER_H_ -#define TRAJECTORY_PROFILE_BUILDER_H_ -#include "cob_cartesian_controller/cartesian_controller_data_types.h" -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h" +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_BUILDER_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_BUILDER_H +#include +#include /* BEGIN TrajectoryProfileBuilder *****************************************************************************************/ class TrajectoryProfileBuilder { - public: - TrajectoryProfileBuilder() {} - ~TrajectoryProfileBuilder() {} - - static TrajectoryProfileBase* createProfile(const cob_cartesian_controller::CartesianActionStruct& params); +public: + TrajectoryProfileBuilder() {} + ~TrajectoryProfileBuilder() {} + + static TrajectoryProfileBase* createProfile(const cob_cartesian_controller::CartesianActionStruct& params); }; /* END TrajectoryGeneratorBuilder *******************************************************************************************/ -#endif +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_BUILDER_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index 6bd6c229..ffcfbee7 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -25,31 +25,34 @@ * Class implementing the Ramp velocity profile generator. * ****************************************************************/ -#ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ -#define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h" +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_RAMP_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_RAMP_H + +#include /* BEGIN TrajectoryProfileRamp ****************************************************************************************/ class TrajectoryProfileRamp: public TrajectoryProfileBase { - public: - TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) - : TrajectoryProfileBase(params) - { } +public: + TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) + : TrajectoryProfileBase(params) + {} + + ~TrajectoryProfileRamp() + {} - ~TrajectoryProfileRamp() {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); - virtual bool generatePath(cob_cartesian_controller::PathArray &pa); - virtual bool calculateProfile(std::vector *path_matrix, - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start); - virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, - double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); + virtual bool generatePath(cob_cartesian_controller::PathArray &pa); + virtual bool calculateProfile(std::vector* path_matrix, + const double Se_lin, const double Se_rot, + geometry_msgs::Pose start); + virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, + double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); - private: - cob_cartesian_controller::ProfileTimings pt_max_; +private: + cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileRamp **********************************************************************************************/ -#endif /* COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H_ */ +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_RAMP_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 45c6ba05..53703774 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -25,31 +25,34 @@ * Class implementing the Sinoid velocity profile generator. * ****************************************************************/ -#ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ -#define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h" +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_SINOID_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_SINOID_H + +#include /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ class TrajectoryProfileSinoid : public TrajectoryProfileBase { - public: - TrajectoryProfileSinoid(const cob_cartesian_controller::CartesianActionStruct& params) - : TrajectoryProfileBase(params) - { } +public: + TrajectoryProfileSinoid(const cob_cartesian_controller::CartesianActionStruct& params) + : TrajectoryProfileBase(params) + {} + + ~TrajectoryProfileSinoid() + {} - ~TrajectoryProfileSinoid() {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); - virtual bool generatePath(cob_cartesian_controller::PathArray &pa); - virtual bool calculateProfile(std::vector* path_matrix, - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start); - virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, - double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); + virtual bool generatePath(cob_cartesian_controller::PathArray& pa); + virtual bool calculateProfile(std::vector* path_matrix, + const double Se_lin, const double Se_rot, + geometry_msgs::Pose start); + virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, + double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); - private: - cob_cartesian_controller::ProfileTimings pt_max_; +private: + cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileSinoid **********************************************************************************************/ -#endif /* COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H_ */ +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_SINOID_H diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index b17a4a9b..cb2863a9 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -27,6 +27,7 @@ * Supported types of velocity profiles are Ramp and Sinoid. * ****************************************************************/ + #include #include #include diff --git a/cob_cartesian_controller/src/cartesian_controller_node.cpp b/cob_cartesian_controller/src/cartesian_controller_node.cpp index e388e5f5..19da4694 100644 --- a/cob_cartesian_controller/src/cartesian_controller_node.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_node.cpp @@ -31,14 +31,14 @@ int main(int argc, char **argv) { - ros::init (argc, argv, "cartesian_controller_node"); - CartesianController cc; + ros::init (argc, argv, "cartesian_controller_node"); + CartesianController cc; - if(!cc.initialize()) - { - ROS_ERROR("Initialization failed"); - return -1; - } + if(!cc.initialize()) + { + ROS_ERROR("Initialization failed"); + return -1; + } - ros::spin(); + ros::spin(); } diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index 37f945a2..2ddb0c2a 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -28,7 +28,6 @@ #include - geometry_msgs::Pose CartesianControllerUtils::getPose(const std::string& target_frame, const std::string& source_frame) { geometry_msgs::Pose pose; @@ -146,9 +145,9 @@ void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray& pose_ double id = marker_array_.markers.size(); - for(unsigned int i=0; i < pose_array.poses.size(); i++) + for(unsigned int i = 0; i < pose_array.poses.size(); i++) { - marker.id = id+i; + marker.id = id + i; marker.pose = pose_array.poses.at(i); marker_array_.markers.push_back(marker); } @@ -156,26 +155,26 @@ void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray& pose_ marker_pub_.publish(marker_array_); } -void CartesianControllerUtils::adjustArrayLength(std::vector &m) +void CartesianControllerUtils::adjustArrayLength(std::vector& m) { unsigned int max_steps = 0; - for(int i = 0; i < m.size(); i++) + for (unsigned int i = 0; i < m.size(); i++) { - max_steps = std::max((unsigned int)m[i].array_.size() , max_steps); + max_steps = std::max((unsigned int)m[i].array_.size(), max_steps); } - for(int i = 0; i < m.size(); i++) + for (unsigned int i = 0; i < m.size(); i++) { - if(m[i].array_.size() < max_steps) + if (m[i].array_.size() < max_steps) { m[i].array_.resize(max_steps, m[i].array_.back()); } } } -void CartesianControllerUtils::copyMatrix(std::vector *path_array,std::vector &m) +void CartesianControllerUtils::copyMatrix(std::vector* path_array, std::vector& m) { - for(int i = 0; i < m.size(); i++) + for (int i = 0; i < m.size(); i++) { path_array[i] = m[i].array_; } @@ -183,5 +182,5 @@ void CartesianControllerUtils::copyMatrix(std::vector *path_array,std::v double CartesianControllerUtils::roundUpToMultiplier(double numberToRound, double multiplier) { - return ( multiplier - std::fmod(numberToRound,multiplier) + numberToRound ); + return ( multiplier - std::fmod(numberToRound, multiplier) + numberToRound ); } diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 737d2b8e..186cf681 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -26,9 +26,10 @@ * ****************************************************************/ +#include + #include #include -#include bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_array, const cob_cartesian_controller::CartesianActionStruct& as) @@ -73,7 +74,7 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ norm_factor = 1/path.back(); // Interpolate the linear path - for(unsigned int i = 0 ; i < linear_path.size() ; i++) + for(unsigned int i = 0; i < linear_path.size(); i++) { if(linear_path.back() == 0) { @@ -112,7 +113,7 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos bool forward; - // Needed for the circle direction ! + // Needed for the circle direction! if(Se < 0) { forward = false; @@ -136,7 +137,7 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos C.setRotation(q); // Interpolate the circular path - for(int i = 0 ; i < path_array.size() ; i++) + for(unsigned int i = 0; i < path_array.size(); i++) { // Rotate T T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp index e4a6863b..4705031b 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp @@ -26,15 +26,12 @@ * ****************************************************************/ -#ifndef COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BUILDER_H_ -#define COB_CONTROL_COB_CARTESIAN_CONTROLLER_INCLUDE_COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BUILDER_H_ +#include +#include +#include +#include -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h" -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h" -#include "cob_cartesian_controller/Profile.h" - -/* BEGIN TrajectoryProfileBase *****************************************************************************************/ +/* BEGIN TrajectoryProfileBuilder *****************************************************************************************/ TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesian_controller::CartesianActionStruct& params) { cob_cartesian_controller::Profile msg; @@ -57,6 +54,4 @@ TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesi return ib; } -/* END TrajectoryProfileBase *******************************************************************************************/ - -#endif +/* END TrajectoryProfileBuilder *******************************************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index f6257417..88be1e22 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -25,8 +25,10 @@ * Class implementing the Ramp velocity profile generator. * ****************************************************************/ -#include "ros/ros.h" -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h" + +#include +#include + /* BEGIN TrajectoryProfileRamp ********************************************************************************************/ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) { @@ -69,8 +71,7 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil return pt; } - -inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray &pa) +inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray& pa) { std::vector array; cob_cartesian_controller::ProfileTimings pt; @@ -100,7 +101,7 @@ inline std::vector TrajectoryProfileRamp::getTrajectory(double se, doubl // Calculate the ramp profile path // 0 <= t <= tb - for(; i <= steps_tb ; i++) + for(; i <= steps_tb; i++) { array.push_back(direction * (0.5*accl*pow((t_ipo*i),2))); } @@ -128,13 +129,13 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat cob_cartesian_controller::PathArray lin(Se_lin, linear_path); cob_cartesian_controller::PathArray rot(Se_rot, angular_path); - cob_cartesian_controller::PathMatrix pm(lin,rot); + cob_cartesian_controller::PathMatrix pm(lin, rot); // Get the profile timings from the longest path pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); // Calculate the paths - for(int i=0; i < pm.pm_.size(); i++) + for(unsigned int i=0; i < pm.pm_.size(); i++) { generatePath(pm.pm_[i]); } @@ -144,9 +145,8 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat // This constant value needs to be duplicated N_max times for matrix conversion purposes. ccu.adjustArrayLength(pm.pm_); - ccu.copyMatrix(path_matrix,pm.pm_); + ccu.copyMatrix(path_matrix, pm.pm_); return true; } - /* END TrajectoryProfileRamp **********************************************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index a89dfd2d..6adc2b88 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -71,7 +71,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf return pt; } - inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) { std::vector array; @@ -136,7 +135,7 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); // Calculate the paths - for(int i=0; i path_m return true; } /* END TrajectoryProfileSinoid ******************************************************************************************/ - From 7b20e12cf282c6acc9ee73de7bca60d3eb48b174 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 14:46:43 +0100 Subject: [PATCH 21/35] roslint cob_cartesian_controller --- cob_cartesian_controller/CMakeLists.txt | 4 +- .../cartesian_controller.h | 8 +- .../cartesian_controller_data_types.h | 165 +++++++++--------- .../cartesian_controller_utils.h | 9 +- .../trajectory_interpolator.h | 8 +- .../trajectory_profile_generator_base.h | 9 +- .../trajectory_profile_generator_builder.h | 8 +- .../trajectory_profile_generator_ramp.h | 9 +- .../trajectory_profile_generator_sinoid.h | 9 +- cob_cartesian_controller/package.xml | 1 + .../src/cartesian_controller.cpp | 93 +++++----- .../src/cartesian_controller_node.cpp | 4 +- .../src/cartesian_controller_utils.cpp | 15 +- .../trajectory_interpolator.cpp | 21 +-- .../trajectory_profile_generator_builder.cpp | 3 +- .../trajectory_profile_generator_ramp.cpp | 21 +-- .../trajectory_profile_generator_sinoid.cpp | 38 ++-- 17 files changed, 220 insertions(+), 205 deletions(-) diff --git a/cob_cartesian_controller/CMakeLists.txt b/cob_cartesian_controller/CMakeLists.txt index 43ea7ac5..d13dbbff 100644 --- a/cob_cartesian_controller/CMakeLists.txt +++ b/cob_cartesian_controller/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(cob_cartesian_controller) -find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib cob_srvs geometry_msgs message_generation roscpp std_msgs std_srvs tf visualization_msgs) +find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib cob_srvs geometry_msgs message_generation roscpp roslint std_msgs std_srvs tf visualization_msgs) find_package(Boost REQUIRED) @@ -53,6 +53,8 @@ target_link_libraries(cartesian_controller trajectory_interpolator cartesian_con add_executable(cartesian_controller_node src/cartesian_controller_node.cpp) target_link_libraries(cartesian_controller_node cartesian_controller ${catkin_LIBRARIES}) +roslint_cpp() + ### INSTALL ## install(TARGETS profile_generator trajectory_interpolator cartesian_controller cartesian_controller_utils cartesian_controller_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h index 996b85df..b6c81a49 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h @@ -28,11 +28,11 @@ * ****************************************************************/ -#ifndef CARTESIAN_CONTROLLER_H -#define CARTESIAN_CONTROLLER_H +#ifndef COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H +#define COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H #include -#include +#include #include #include @@ -100,4 +100,4 @@ class CartesianController boost::shared_ptr< TrajectoryInterpolator > trajectory_interpolator_; }; -#endif // CARTESIAN_CONTROLLER_H +#endif // COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 8d2ce26d..8cb9db7e 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -26,103 +26,104 @@ * ****************************************************************/ -#ifndef CARTESIAN_CONTROLLER_DATA_STRUCTURES_H -#define CARTESIAN_CONTROLLER_DATA_STRUCTURES_H +#ifndef COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_DATA_TYPES_H +#define COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_DATA_TYPES_H -#include +#include #include #include namespace cob_cartesian_controller { - struct ProfileStruct - { - double t_ipo; - unsigned int profile_type; - double vel, accl; - double Se_max; - }; - - struct ProfileTimings - { - double tb, te, tv; - unsigned int steps_tb, steps_te, steps_tv; - bool ok; - double vel; - }; - - struct MoveLinStruct - { - geometry_msgs::Pose start, end; - }; - - struct MoveCircStruct - { - geometry_msgs::Pose pose_center; - double start_angle, end_angle; - double radius; - }; - - struct CartesianActionStruct - { - unsigned int move_type; - MoveLinStruct move_lin; - MoveCircStruct move_circ; - ProfileStruct profile; - }; - - class PathArray - { - public: - PathArray(double Se, std::vector array): - Se_(Se), - array_(array) - { - calcTe_ = false; - } - ~PathArray() - { - array_.clear(); - } +struct ProfileStruct +{ + double t_ipo; + unsigned int profile_type; + double vel, accl; + double Se_max; +}; - bool calcTe_; - double Se_; - std::vector array_; - }; +struct ProfileTimings +{ + double tb, te, tv; + unsigned int steps_tb, steps_te, steps_tv; + bool ok; + double vel; +}; - class PathMatrix - { - public: - PathMatrix(PathArray& pa1, - PathArray& pa2) - { - pm_.push_back(pa1); - pm_.push_back(pa2); - } +struct MoveLinStruct +{ + geometry_msgs::Pose start, end; +}; - ~PathMatrix() - { - pm_.clear(); - } +struct MoveCircStruct +{ + geometry_msgs::Pose pose_center; + double start_angle, end_angle; + double radius; +}; - double getMaxSe() - { - double se_max = 0; +struct CartesianActionStruct +{ + unsigned int move_type; + MoveLinStruct move_lin; + MoveCircStruct move_circ; + ProfileStruct profile; +}; - for (unsigned int i = 0; i < pm_.size(); i++) +class PathArray +{ + public: + PathArray(double Se, std::vector array): + Se_(Se), + array_(array) + { + calcTe_ = false; + } + + ~PathArray() + { + array_.clear(); + } + + bool calcTe_; + double Se_; + std::vector array_; +}; + +class PathMatrix +{ + public: + PathMatrix(PathArray& pa1, + PathArray& pa2) + { + pm_.push_back(pa1); + pm_.push_back(pa2); + } + + ~PathMatrix() + { + pm_.clear(); + } + + double getMaxSe() + { + double se_max = 0; + + for (unsigned int i = 0; i < pm_.size(); i++) + { + if (se_max < fabs(pm_[i].Se_)) { - if (se_max < fabs(pm_[i].Se_)) - { - se_max = fabs(pm_[i].Se_); - } + se_max = fabs(pm_[i].Se_); } - return se_max; } + return se_max; + } - std::vector pm_; - }; + std::vector pm_; +}; -}// namespace +} // namespace cob_cartesian_controller -#endif // CARTESIAN_CONTROLLER_DATA_STRUCTURES_H +#endif // COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_DATA_TYPES_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h index e167245f..37756849 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h @@ -26,8 +26,11 @@ * ****************************************************************/ -#ifndef CARTESIAN_CONTROLLER_UTILS_H -#define CARTESIAN_CONTROLLER_UTILS_H +#ifndef COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H +#define COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H + +#include +#include #include #include @@ -67,4 +70,4 @@ class CartesianControllerUtils ros::Publisher marker_pub_; }; -#endif // CARTESIAN_CONTROLLER_UTILS_H +#endif // COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h index 4d61c2d2..3417a1a7 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h @@ -26,9 +26,10 @@ * ****************************************************************/ -#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H -#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_TRAJECTORY_INTERPOLATOR_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_TRAJECTORY_INTERPOLATOR_H +#include #include #include #include @@ -57,7 +58,6 @@ class TrajectoryInterpolator private: std::string root_frame_; boost::shared_ptr trajectory_profile_generator_; - }; -#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_TRAJECTORY_INTERPOLATOR_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 922a9aa3..453aab18 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -26,9 +26,10 @@ * ****************************************************************/ -#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H -#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BASE_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BASE_H +#include #include #include #include @@ -39,7 +40,7 @@ class TrajectoryProfileBase { public: - TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct& params) + explicit TrajectoryProfileBase(const cob_cartesian_controller::CartesianActionStruct& params) : params_(params) {} @@ -61,4 +62,4 @@ class TrajectoryProfileBase const cob_cartesian_controller::CartesianActionStruct& params_; }; -#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_BASE_H +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BASE_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h index 5496cd41..8b78f7a5 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h @@ -26,8 +26,8 @@ * ****************************************************************/ -#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_BUILDER_H -#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_BUILDER_H +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BUILDER_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BUILDER_H #include #include @@ -38,9 +38,9 @@ class TrajectoryProfileBuilder public: TrajectoryProfileBuilder() {} ~TrajectoryProfileBuilder() {} - + static TrajectoryProfileBase* createProfile(const cob_cartesian_controller::CartesianActionStruct& params); }; /* END TrajectoryGeneratorBuilder *******************************************************************************************/ -#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_BUILDER_H +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BUILDER_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index ffcfbee7..08214f45 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -26,16 +26,17 @@ * ****************************************************************/ -#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_RAMP_H -#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_RAMP_H +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H +#include #include /* BEGIN TrajectoryProfileRamp ****************************************************************************************/ class TrajectoryProfileRamp: public TrajectoryProfileBase { public: - TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) + explicit TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) : TrajectoryProfileBase(params) {} @@ -55,4 +56,4 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase }; /* END TrajectoryProfileRamp **********************************************************************************************/ -#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_RAMP_H +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_RAMP_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 53703774..81985cf7 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -26,16 +26,17 @@ * ****************************************************************/ -#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_SINOID_H -#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_SINOID_H +#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H +#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H +#include #include /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ class TrajectoryProfileSinoid : public TrajectoryProfileBase { public: - TrajectoryProfileSinoid(const cob_cartesian_controller::CartesianActionStruct& params) + explicit TrajectoryProfileSinoid(const cob_cartesian_controller::CartesianActionStruct& params) : TrajectoryProfileBase(params) {} @@ -55,4 +56,4 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase }; /* END TrajectoryProfileSinoid **********************************************************************************************/ -#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_SINOID_H +#endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_SINOID_H diff --git a/cob_cartesian_controller/package.xml b/cob_cartesian_controller/package.xml index a123f61c..9f11a894 100644 --- a/cob_cartesian_controller/package.xml +++ b/cob_cartesian_controller/package.xml @@ -12,6 +12,7 @@ LGPL catkin + roslint message_generation message_runtime diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index cb2863a9..efe9e71b 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include @@ -48,25 +49,24 @@ bool CartesianController::initialize() return false; } - if(!nh_.getParam("root_frame", root_frame_)) + if (!nh_.getParam("root_frame", root_frame_)) { ROS_ERROR("Parameter 'reference_frame' not set"); return false; } - if(!nh_.getParam("update_rate", update_rate_)) + if (!nh_.getParam("update_rate", update_rate_)) { - update_rate_ = 50.0; //hz + update_rate_ = 50.0; // hz } /// Private Nodehandle - if(!nh_private.getParam("target_frame", target_frame_)) + if (!nh_private.getParam("target_frame", target_frame_)) { ROS_WARN("Parameter 'target_frame' not set. Using default 'cartesian_target'"); target_frame_ = DEFAULT_CARTESIAN_TARGET; } - ROS_WARN("Waiting for Services..."); start_tracking_ = nh_.serviceClient("frame_tracker/start_tracking"); stop_tracking_ = nh_.serviceClient("frame_tracker/stop"); @@ -86,21 +86,20 @@ bool CartesianController::initialize() return true; } - -//ToDo: Use the ActionInterface of the FrameTracker instead in order to be able to consider TrackingConstraints +// ToDo: Use the ActionInterface of the FrameTracker instead in order to be able to consider TrackingConstraints bool CartesianController::startTracking() { bool success = false; cob_srvs::SetString start; start.request.data = target_frame_; - if(!tracking_) + if (!tracking_) { success = start_tracking_.call(start); - if(success) + if (success) { success = start.response.success; - if(success) + if (success) { ROS_INFO("Response 'start_tracking': succeded"); tracking_ = true; @@ -123,16 +122,16 @@ bool CartesianController::startTracking() return success; } -//ToDo:: If we use the ActionInterface of the FrameTracker, here that action should be cancled() +// ToDo:: If we use the ActionInterface of the FrameTracker, here that action should be cancled() bool CartesianController::stopTracking() { bool success = false; std_srvs::Trigger stop; - if(tracking_) + if (tracking_) { success = stop_tracking_.call(stop); - if(success) + if (success) { ROS_INFO("Service 'stop' succeded!"); tracking_ = false; @@ -150,7 +149,6 @@ bool CartesianController::stopTracking() return success; } - // MovePTP bool CartesianController::movePTP(const geometry_msgs::Pose& target_pose, const double epsilon) { @@ -159,17 +157,17 @@ bool CartesianController::movePTP(const geometry_msgs::Pose& target_pose, const ros::Rate rate(update_rate_); tf::StampedTransform stamped_transform; - stamped_transform.setOrigin( tf::Vector3(target_pose.position.x, - target_pose.position.y, - target_pose.position.z) ); - stamped_transform.setRotation( tf::Quaternion(target_pose.orientation.x, - target_pose.orientation.y, - target_pose.orientation.z, - target_pose.orientation.w) ); + stamped_transform.setOrigin(tf::Vector3(target_pose.position.x, + target_pose.position.y, + target_pose.position.z)); + stamped_transform.setRotation(tf::Quaternion(target_pose.orientation.x, + target_pose.orientation.y, + target_pose.orientation.z, + target_pose.orientation.w)); stamped_transform.frame_id_ = root_frame_; stamped_transform.child_frame_id_ = target_frame_; - while(as_->isActive()) + while (as_->isActive()) { // Send/Refresh target Frame stamped_transform.stamp_ = ros::Time::now(); @@ -179,12 +177,12 @@ bool CartesianController::movePTP(const geometry_msgs::Pose& target_pose, const tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); // Wait for chain_tip_link to be within epsilon area of target_frame - if(utils_.inEpsilonArea(stamped_transform, epsilon)) + if (utils_.inEpsilonArea(stamped_transform, epsilon)) { - reached_pos_counter++; // Count up if end effector position is in the epsilon area to avoid wrong values + reached_pos_counter++; // Count up if end effector position is in the epsilon area to avoid wrong values } - if(reached_pos_counter >= static_cast(2*update_rate_)) //has been close enough to target for 2 seconds + if (reached_pos_counter >= static_cast(2*update_rate_)) // has been close enough to target for 2 seconds { success = true; break; @@ -206,22 +204,22 @@ bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& ca ros::Rate rate(update_rate_); tf::Transform transform; - for(unsigned int i = 0; i < cartesian_path.poses.size(); i++) + for (unsigned int i = 0; i < cartesian_path.poses.size(); i++) { - if(!as_->isActive()) + if (!as_->isActive()) { success = false; break; } // Send/Refresh target Frame - transform.setOrigin( tf::Vector3(cartesian_path.poses.at(i).position.x, - cartesian_path.poses.at(i).position.y, - cartesian_path.poses.at(i).position.z) ); - transform.setRotation( tf::Quaternion(cartesian_path.poses.at(i).orientation.x, - cartesian_path.poses.at(i).orientation.y, - cartesian_path.poses.at(i).orientation.z, - cartesian_path.poses.at(i).orientation.w) ); + transform.setOrigin(tf::Vector3(cartesian_path.poses.at(i).position.x, + cartesian_path.poses.at(i).position.y, + cartesian_path.poses.at(i).position.z)); + transform.setRotation(tf::Quaternion(cartesian_path.poses.at(i).orientation.x, + cartesian_path.poses.at(i).orientation.y, + cartesian_path.poses.at(i).orientation.z, + cartesian_path.poses.at(i).orientation.w)); tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cartesian_path.header.frame_id, target_frame_)); @@ -240,10 +238,10 @@ void CartesianController::goalCallback() action_struct = acceptGoal(as_->acceptNewGoal()); - if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::LIN) + if (action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::LIN) { // Interpolate path - if(!trajectory_interpolator_->linearInterpolation(cartesian_path, action_struct)) + if (!trajectory_interpolator_->linearInterpolation(cartesian_path, action_struct)) { actionAbort(false, "Failed to do interpolation for 'move_lin'"); return; @@ -253,21 +251,21 @@ void CartesianController::goalCallback() utils_.previewPath(cartesian_path); // Activate Tracking - if(!startTracking()) + if (!startTracking()) { actionAbort(false, "Failed to start tracking"); return; } // Execute path - if(!posePathBroadcaster(cartesian_path)) + if (!posePathBroadcaster(cartesian_path)) { actionAbort(false, "Failed to execute path for 'move_lin'"); return; } // De-Activate Tracking - if(!stopTracking()) + if (!stopTracking()) { actionAbort(false, "Failed to stop tracking"); return; @@ -275,9 +273,9 @@ void CartesianController::goalCallback() actionSuccess(true, "move_lin succeeded!"); } - else if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::CIRC) + else if (action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::CIRC) { - if(!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct)) + if (!trajectory_interpolator_->circularInterpolation(cartesian_path, action_struct)) { actionAbort(false, "Failed to do interpolation for 'move_circ'"); return; @@ -287,21 +285,21 @@ void CartesianController::goalCallback() utils_.previewPath(cartesian_path); // Activate Tracking - if(!startTracking()) + if (!startTracking()) { actionAbort(false, "Failed to start tracking"); return; } // Execute path - if(!posePathBroadcaster(cartesian_path)) + if (!posePathBroadcaster(cartesian_path)) { actionAbort(false, "Failed to execute path for 'move_circ'"); return; } // De-Activate Tracking - if(!stopTracking()) + if (!stopTracking()) { actionAbort(false, "Failed to stop tracking"); return; @@ -319,7 +317,7 @@ void CartesianController::goalCallback() cob_cartesian_controller::MoveLinStruct CartesianController::convertMoveLin(const cob_cartesian_controller::MoveLin& move_lin_msg) { geometry_msgs::Pose start, end; - start = utils_.getPose(root_frame_, chain_tip_link_); //current tcp pose + start = utils_.getPose(root_frame_, chain_tip_link_); // current tcp pose utils_.transformPose(move_lin_msg.frame_id, root_frame_, move_lin_msg.pose_goal, end); cob_cartesian_controller::MoveLinStruct move_lin; @@ -356,17 +354,16 @@ cob_cartesian_controller::CartesianActionStruct CartesianController::acceptGoal( action_struct.profile.t_ipo = 1/update_rate_; - if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::LIN) + if (action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::LIN) { action_struct.move_lin = convertMoveLin(goal->move_lin); } - else if(action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::CIRC) + else if (action_struct.move_type == cob_cartesian_controller::CartesianControllerGoal::CIRC) { action_struct.move_circ = convertMoveCirc(goal->move_circ); } else { - actionAbort(false, "Unknown trajectory action " + boost::lexical_cast(action_struct.move_type)); } diff --git a/cob_cartesian_controller/src/cartesian_controller_node.cpp b/cob_cartesian_controller/src/cartesian_controller_node.cpp index 19da4694..f5f60da9 100644 --- a/cob_cartesian_controller/src/cartesian_controller_node.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_node.cpp @@ -31,10 +31,10 @@ int main(int argc, char **argv) { - ros::init (argc, argv, "cartesian_controller_node"); + ros::init(argc, argv, "cartesian_controller_node"); CartesianController cc; - if(!cc.initialize()) + if (!cc.initialize()) { ROS_ERROR("Initialization failed"); return -1; diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index 2ddb0c2a..dd0dcddb 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -26,6 +26,9 @@ * ****************************************************************/ +#include +#include +#include #include geometry_msgs::Pose CartesianControllerUtils::getPose(const std::string& target_frame, const std::string& source_frame) @@ -57,10 +60,10 @@ tf::StampedTransform CartesianControllerUtils::getStampedTransform(const std::st } catch (tf::TransformException& ex) { - ROS_ERROR("CartesianControllerUtils::getStampedTransform: \n%s",ex.what()); + ROS_ERROR("CartesianControllerUtils::getStampedTransform: \n%s", ex.what()); ros::Duration(0.1).sleep(); } - }while(!transform && ros::ok()); + } while (!transform && ros::ok()); return stamped_transform; } @@ -84,10 +87,10 @@ void CartesianControllerUtils::transformPose(const std::string source_frame, con } catch (tf::TransformException& ex) { - ROS_ERROR("CartesianControllerUtils::transformPose: \n%s",ex.what()); + ROS_ERROR("CartesianControllerUtils::transformPose: \n%s", ex.what()); ros::Duration(0.1).sleep(); } - }while(!transform && ros::ok()); + } while (!transform && ros::ok()); } @@ -110,7 +113,7 @@ bool CartesianControllerUtils::inEpsilonArea(const tf::StampedTransform& stamped pitch_okay = std::fabs(pitch) < epsilon; yaw_okay = std::fabs(yaw) < epsilon; - if(x_okay && y_okay && z_okay && roll_okay && pitch_okay && yaw_okay) + if (x_okay && y_okay && z_okay && roll_okay && pitch_okay && yaw_okay) { return true; } @@ -145,7 +148,7 @@ void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray& pose_ double id = marker_array_.markers.size(); - for(unsigned int i = 0; i < pose_array.poses.size(); i++) + for (unsigned int i = 0; i < pose_array.poses.size(); i++) { marker.id = id + i; marker.pose = pose_array.poses.at(i); diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 186cf681..26582ed3 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -27,6 +27,7 @@ ****************************************************************/ #include +#include #include #include @@ -46,8 +47,8 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ geometry_msgs::Pose pose; double norm_factor; - tf::quaternionMsgToTF(as.move_lin.start.orientation,q_start); - tf::quaternionMsgToTF(as.move_lin.end.orientation,q_end); + tf::quaternionMsgToTF(as.move_lin.start.orientation, q_start); + tf::quaternionMsgToTF(as.move_lin.end.orientation, q_end); double Se_lin = sqrt(pow((as.move_lin.end.position.x - as.move_lin.start.position.x), 2) + pow((as.move_lin.end.position.y - as.move_lin.start.position.y), 2) + @@ -55,7 +56,7 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ double Se_rot = q_start.angleShortestPath(q_end); - if(!trajectory_profile_generator_->calculateProfile(path_matrix, Se_lin, Se_rot, as.move_lin.start)) + if (!trajectory_profile_generator_->calculateProfile(path_matrix, Se_lin, Se_rot, as.move_lin.start)) { return false; } @@ -63,7 +64,7 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ linear_path = path_matrix[0]; angular_path = path_matrix[1]; - if(fabs(linear_path.back()) > fabs(angular_path.back())) + if (fabs(linear_path.back()) > fabs(angular_path.back())) { path = linear_path; } @@ -74,9 +75,9 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ norm_factor = 1/path.back(); // Interpolate the linear path - for(unsigned int i = 0; i < linear_path.size(); i++) + for (unsigned int i = 0; i < linear_path.size(); i++) { - if(linear_path.back() == 0) + if (linear_path.back() == 0) { pose.position.x = as.move_lin.start.position.x; pose.position.y = as.move_lin.start.position.y; @@ -114,7 +115,7 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos bool forward; // Needed for the circle direction! - if(Se < 0) + if (Se < 0) { forward = false; } @@ -125,7 +126,7 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos Se = std::fabs(Se); // Calculates the Path with RAMP or SINOID profile - if(!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0, pose)) + if (!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0, pose)) { return false; } @@ -137,12 +138,12 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos C.setRotation(q); // Interpolate the circular path - for(unsigned int i = 0; i < path_array.size(); i++) + for (unsigned int i = 0; i < path_array.size(); i++) { // Rotate T T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); - if(forward) + if (forward) { T.setOrigin(tf::Vector3(cos(path_array.at(i)) * as.move_circ.radius, 0, sin(path_array.at(i)) * as.move_circ.radius)); q.setRPY(0, -path_array.at(i), 0); diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp index 4705031b..a745399e 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp @@ -38,8 +38,9 @@ TrajectoryProfileBase* TrajectoryProfileBuilder::createProfile(const cob_cartesi const int RAMP = static_cast(msg.RAMP); const int SINOID = static_cast(msg.SINOID); + TrajectoryProfileBase* ib = NULL; - switch(params.profile.profile_type) + switch (params.profile.profile_type) { case RAMP: ib = new TrajectoryProfileRamp(params); diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index 88be1e22..dcfe144f 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -26,6 +26,7 @@ * ****************************************************************/ +#include #include #include @@ -42,10 +43,10 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil vel = sqrt(std::fabs(Se) * accl); } - if(vel > MIN_VELOCITY_THRESHOLD) + if (vel > MIN_VELOCITY_THRESHOLD) { tb = utils.roundUpToMultiplier(vel / accl, params_.profile.t_ipo); - if(calcMaxTe) + if (calcMaxTe) { te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb, params_.profile.t_ipo); } @@ -80,7 +81,7 @@ inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathAr // Calculate the Profile Timings pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); - if(pt.ok) + if (pt.ok) { array = getTrajectory(pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } @@ -101,19 +102,19 @@ inline std::vector TrajectoryProfileRamp::getTrajectory(double se, doubl // Calculate the ramp profile path // 0 <= t <= tb - for(; i <= steps_tb; i++) + for (; i <= steps_tb; i++) { - array.push_back(direction * (0.5*accl*pow((t_ipo*i),2))); + array.push_back(direction * (0.5*accl*pow((t_ipo*i), 2))); } // tb <= t <= tv - for(; i <= (steps_tb + steps_tv); i++) + for (; i <= (steps_tb + steps_tv); i++) { - array.push_back(direction *(vel*(t_ipo*i) - 0.5*pow(vel,2)/accl)); + array.push_back(direction *(vel*(t_ipo*i) - 0.5*pow(vel, 2)/accl)); } // tv <= t <= te - for(; i <= (steps_tb + steps_tv + steps_te + 1); i++) + for (; i <= (steps_tb + steps_tv + steps_te + 1); i++) { - array.push_back(direction * (vel * tv - 0.5 * accl * pow(te-(t_ipo*i),2))); + array.push_back(direction * (vel * tv - 0.5 * accl * pow(te-(t_ipo*i), 2))); } return array; @@ -135,7 +136,7 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); // Calculate the paths - for(unsigned int i=0; i < pm.pm_.size(); i++) + for (unsigned int i=0; i < pm.pm_.size(); i++) { generatePath(pm.pm_[i]); } diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index 6adc2b88..9cddc455 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -25,8 +25,10 @@ * Class implementing the Sinoid velocity profile generator. * ****************************************************************/ -#include "ros/ros.h" -#include "cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h" + +#include +#include +#include /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) @@ -41,11 +43,11 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf vel = sqrt(std::fabs(Se) * accl / 2); } - if(vel > MIN_VELOCITY_THRESHOLD) + if (vel > MIN_VELOCITY_THRESHOLD) { tb = utils.roundUpToMultiplier(2 * vel / accl, params_.profile.t_ipo); - if(te == 0) + if (te == 0) { te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb, params_.profile.t_ipo); } @@ -71,7 +73,7 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf return pt; } -inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray &pa) +inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray& pa) { std::vector array; cob_cartesian_controller::ProfileTimings pt; @@ -80,7 +82,7 @@ inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::Path // Calculate the Profile Timings pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); - if(pt.ok) + if (pt.ok) { array = getTrajectory(pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } @@ -99,29 +101,29 @@ inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, dou unsigned int i = 1; double direction = se/std::fabs(se); - //Calculate the sinoid profile path + // Calculate the sinoid profile path // 0 <= t <= tb - for(; i <= steps_tb; i++) + for (; i <= steps_tb; i++) { - array.push_back(direction * (accl*(0.25*pow(i*t_ipo,2) + pow(tb,2)/(8*pow(M_PI,2)) *(cos(2*M_PI/tb * (i*t_ipo))-1)))); + array.push_back(direction * (accl*(0.25*pow(i*t_ipo, 2) + pow(tb, 2)/(8*pow(M_PI, 2)) *(cos(2*M_PI/tb * (i*t_ipo))-1)))); } // tb <= t <= tv - for(; i <= (steps_tb + steps_tv); i++) + for (; i <= (steps_tb + steps_tv); i++) { - array.push_back(direction * ( vel*(i*t_ipo-0.5*tb))); + array.push_back(direction * (vel*(i*t_ipo-0.5*tb))); } // tv <= t <= te - for(; i <= (steps_tv + steps_tb + steps_te + 1); i++) + for (; i <= (steps_tv + steps_tb + steps_te + 1); i++) { - array.push_back(direction * (0.5 * accl *(te*(i*t_ipo + tb) - 0.5*(pow(i*t_ipo,2)+pow(te,2)+2*pow(tb,2)) + (pow(tb,2)/(4*pow(M_PI,2))) * (1-cos(((2*M_PI)/tb) * (i*t_ipo-tv)))))); + array.push_back(direction * (0.5 * accl *(te*(i*t_ipo + tb) - 0.5*(pow(i*t_ipo, 2)+pow(te, 2)+2*pow(tb, 2)) + (pow(tb, 2)/(4*pow(M_PI, 2))) * (1-cos(((2*M_PI)/tb) * (i*t_ipo-tv)))))); } return array; } inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_matrix[2], - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start) + const double Se_lin, const double Se_rot, + geometry_msgs::Pose start) { CartesianControllerUtils ccu; std::vector linear_path, angular_path; @@ -129,13 +131,13 @@ inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_m cob_cartesian_controller::PathArray lin(Se_lin, linear_path); cob_cartesian_controller::PathArray rot(Se_rot, angular_path); - cob_cartesian_controller::PathMatrix pm(lin,rot); + cob_cartesian_controller::PathMatrix pm(lin, rot); // Get the profile timings from the longest path pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); // Calculate the paths - for(unsigned int i=0; i path_m // This constant value needs to be duplicated N_max times for matrix conversion purposes. ccu.adjustArrayLength(pm.pm_); - ccu.copyMatrix(path_matrix,pm.pm_); + ccu.copyMatrix(path_matrix, pm.pm_); return true; } From f6864c74d78d61927dc1a776e019ded189563647 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 14:47:27 +0100 Subject: [PATCH 22/35] roslint cob_twist_controller --- cob_twist_controller/src/debug/test_moving_average_node.cpp | 1 + .../src/debug/test_simpson_integrator_node.cpp | 1 + .../src/kinematic_extensions/kinematic_extension_dof.cpp | 1 + .../src/kinematic_extensions/kinematic_extension_lookat.cpp | 2 ++ .../src/kinematic_extensions/kinematic_extension_urdf.cpp | 1 + cob_twist_controller/src/limiters/limiter.cpp | 4 ++-- 6 files changed, 8 insertions(+), 2 deletions(-) diff --git a/cob_twist_controller/src/debug/test_moving_average_node.cpp b/cob_twist_controller/src/debug/test_moving_average_node.cpp index 22da7942..a74cbb3d 100644 --- a/cob_twist_controller/src/debug/test_moving_average_node.cpp +++ b/cob_twist_controller/src/debug/test_moving_average_node.cpp @@ -1,3 +1,4 @@ +#include #include #include #include diff --git a/cob_twist_controller/src/debug/test_simpson_integrator_node.cpp b/cob_twist_controller/src/debug/test_simpson_integrator_node.cpp index f40df855..a3b6e6b2 100644 --- a/cob_twist_controller/src/debug/test_simpson_integrator_node.cpp +++ b/cob_twist_controller/src/debug/test_simpson_integrator_node.cpp @@ -1,3 +1,4 @@ +#include #include #include #include diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp index e2ae45d9..01e47255 100644 --- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp +++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp @@ -27,6 +27,7 @@ * ****************************************************************/ +#include #include #include "cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h" diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp index aee977be..3085bd18 100644 --- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp +++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp @@ -28,6 +28,8 @@ ****************************************************************/ #include +#include +#include #include #include "cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h" diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp index 7f64e621..27100ca5 100644 --- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp +++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp @@ -28,6 +28,7 @@ ****************************************************************/ #include +#include #include #include "cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h" diff --git a/cob_twist_controller/src/limiters/limiter.cpp b/cob_twist_controller/src/limiters/limiter.cpp index effd01f1..3c259fe5 100644 --- a/cob_twist_controller/src/limiters/limiter.cpp +++ b/cob_twist_controller/src/limiters/limiter.cpp @@ -144,7 +144,7 @@ KDL::JntArray LimiterAllJointPositions::enforceLimits(const KDL::JntArray& q_dot if (q_dot_ik(i) > 0) // Joint moves towards the MAX limit { double temp = 1.0 / pow((0.5 + 0.5 * cos(M_PI * (q(i) + tolerance - limiter_params_.limits_max[i]) / tolerance)), 5.0); - if(temp > max_factor) + if (temp > max_factor) { max_factor = temp; joint_index = i; @@ -158,7 +158,7 @@ KDL::JntArray LimiterAllJointPositions::enforceLimits(const KDL::JntArray& q_dot if (q_dot_ik(i) < 0) // Joint moves towards the MIN limit { double temp = 1.0 / pow(0.5 + 0.5 * cos(M_PI * (q(i) - tolerance - limiter_params_.limits_min[i]) / tolerance), 5.0); - if(temp > max_factor) + if (temp > max_factor) { max_factor = temp; joint_index = i; From e0d6642d73e75686e8cb1c025a7b6319aa896c63 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 14:53:54 +0100 Subject: [PATCH 23/35] minor styling --- cob_cartesian_controller/src/cartesian_controller_utils.cpp | 2 +- .../trajectory_profile_generator_ramp.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index dd0dcddb..7dc1642a 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -177,7 +177,7 @@ void CartesianControllerUtils::adjustArrayLength(std::vector* path_array, std::vector& m) { - for (int i = 0; i < m.size(); i++) + for (unsigned int i = 0; i < m.size(); i++) { path_array[i] = m[i].array_; } diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index dcfe144f..ea740058 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -136,7 +136,7 @@ inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_mat pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); // Calculate the paths - for (unsigned int i=0; i < pm.pm_.size(); i++) + for (unsigned int i = 0; i < pm.pm_.size(); i++) { generatePath(pm.pm_[i]); } From fb6102a6443ffbdc62467519cdbce304e048f11d Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 14:55:37 +0100 Subject: [PATCH 24/35] remove obsolete calcTe_ --- .../cartesian_controller_data_types.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 8cb9db7e..2c34e908 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -78,16 +78,13 @@ class PathArray PathArray(double Se, std::vector array): Se_(Se), array_(array) - { - calcTe_ = false; - } + {} ~PathArray() { array_.clear(); } - bool calcTe_; double Se_; std::vector array_; }; From e839b15d025bd76350a126fae10fcd4484f6a60d Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 15:02:19 +0100 Subject: [PATCH 25/35] harmonizing --- .../trajectory_profile_generator_sinoid.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index 9cddc455..0fd2cca7 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -46,7 +46,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf if (vel > MIN_VELOCITY_THRESHOLD) { tb = utils.roundUpToMultiplier(2 * vel / accl, params_.profile.t_ipo); - if (te == 0) { te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb, params_.profile.t_ipo); From 5a618771be8f9e7edce4605284485f5ebf677df2 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 15:21:50 +0100 Subject: [PATCH 26/35] move identical functions to base class --- .../trajectory_profile_generator_base.h | 59 +++++++++++++++++-- .../trajectory_profile_generator_ramp.h | 7 --- .../trajectory_profile_generator_sinoid.h | 7 --- .../trajectory_profile_generator_ramp.cpp | 53 ----------------- .../trajectory_profile_generator_sinoid.cpp | 53 ----------------- 5 files changed, 53 insertions(+), 126 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 453aab18..8895f147 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -47,19 +47,66 @@ class TrajectoryProfileBase virtual ~TrajectoryProfileBase() {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) = 0; - virtual bool calculateProfile(std::vector* path_matrix, const double Se_lin, const double Se_rot, - geometry_msgs::Pose start) = 0; + geometry_msgs::Pose start) + { + CartesianControllerUtils ccu; + std::vector linear_path, angular_path; + + cob_cartesian_controller::PathArray lin(Se_lin, linear_path); + cob_cartesian_controller::PathArray rot(Se_rot, angular_path); + + cob_cartesian_controller::PathMatrix pm(lin, rot); + + // Get the profile timings from the longest path + pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); + + // Calculate the paths + for (unsigned int i = 0; i < pm.pm_.size(); i++) + { + generatePath(pm.pm_[i]); + } + + // Adjust the array length + // If no trajectory was interpolated, then this path array contains only one constant value. + // This constant value needs to be duplicated N_max times for matrix conversion purposes. + ccu.adjustArrayLength(pm.pm_); + ccu.copyMatrix(path_matrix, pm.pm_); + + return true; + } + +protected: + virtual bool generatePath(cob_cartesian_controller::PathArray& pa) + { + std::vector array; + cob_cartesian_controller::ProfileTimings pt; + double accl_max = params_.profile.accl; + double vel_max = params_.profile.vel; + + // Calculate the Profile Timings + pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); + if (pt.ok) + { + array = getTrajectory(pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); + } + else + { + array.push_back(0); + } + + pa.array_ = array; + return true; + } + + virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) = 0; virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; -private: - virtual bool generatePath(cob_cartesian_controller::PathArray& pa) = 0; -protected: const cob_cartesian_controller::CartesianActionStruct& params_; + cob_cartesian_controller::ProfileTimings pt_max_; }; #endif // COB_CARTESIAN_CONTROLLER_TRAJECTORY_PROFILE_GENERATOR_TRAJECTORY_PROFILE_GENERATOR_BASE_H diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index 08214f45..494ee3e9 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -44,15 +44,8 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase {} virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); - virtual bool generatePath(cob_cartesian_controller::PathArray &pa); - virtual bool calculateProfile(std::vector* path_matrix, - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start); virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); - -private: - cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileRamp **********************************************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 81985cf7..78553079 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -44,15 +44,8 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase {} virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); - virtual bool generatePath(cob_cartesian_controller::PathArray& pa); - virtual bool calculateProfile(std::vector* path_matrix, - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start); virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); - -private: - cob_cartesian_controller::ProfileTimings pt_max_; }; /* END TrajectoryProfileSinoid **********************************************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index ea740058..49b534a2 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -72,28 +72,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil return pt; } -inline bool TrajectoryProfileRamp::generatePath(cob_cartesian_controller::PathArray& pa) -{ - std::vector array; - cob_cartesian_controller::ProfileTimings pt; - double accl_max = params_.profile.accl; - double vel_max = params_.profile.vel; - - // Calculate the Profile Timings - pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); - if (pt.ok) - { - array = getTrajectory(pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); - } - else - { - array.push_back(0); - } - - pa.array_ = array; - return true; -} - inline std::vector TrajectoryProfileRamp::getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) { std::vector array; @@ -119,35 +97,4 @@ inline std::vector TrajectoryProfileRamp::getTrajectory(double se, doubl return array; } - -inline bool TrajectoryProfileRamp::calculateProfile(std::vector path_matrix[2], - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start) -{ - CartesianControllerUtils ccu; - std::vector linear_path, angular_path; - - cob_cartesian_controller::PathArray lin(Se_lin, linear_path); - cob_cartesian_controller::PathArray rot(Se_rot, angular_path); - - cob_cartesian_controller::PathMatrix pm(lin, rot); - - // Get the profile timings from the longest path - pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); - - // Calculate the paths - for (unsigned int i = 0; i < pm.pm_.size(); i++) - { - generatePath(pm.pm_[i]); - } - - // Adjust the array length - // If no trajectory was interpolated, then this path array contains only one constant value. - // This constant value needs to be duplicated N_max times for matrix conversion purposes. - ccu.adjustArrayLength(pm.pm_); - - ccu.copyMatrix(path_matrix, pm.pm_); - - return true; -} /* END TrajectoryProfileRamp **********************************************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index 0fd2cca7..1c3619c7 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -72,28 +72,6 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf return pt; } -inline bool TrajectoryProfileSinoid::generatePath(cob_cartesian_controller::PathArray& pa) -{ - std::vector array; - cob_cartesian_controller::ProfileTimings pt; - double accl_max = params_.profile.accl; - double vel_max = params_.profile.vel; - - // Calculate the Profile Timings - pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); - if (pt.ok) - { - array = getTrajectory(pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); - } - else - { - array.push_back(0); - } - - pa.array_ = array; - return true; -} - inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) { std::vector array; @@ -119,35 +97,4 @@ inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, dou return array; } - -inline bool TrajectoryProfileSinoid::calculateProfile(std::vector path_matrix[2], - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start) -{ - CartesianControllerUtils ccu; - std::vector linear_path, angular_path; - - cob_cartesian_controller::PathArray lin(Se_lin, linear_path); - cob_cartesian_controller::PathArray rot(Se_rot, angular_path); - - cob_cartesian_controller::PathMatrix pm(lin, rot); - - // Get the profile timings from the longest path - pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); - - // Calculate the paths - for (unsigned int i = 0; i < pm.pm_.size(); i++) - { - generatePath(pm.pm_[i]); - } - - // Adjust the array length - // If no trajectory was interpolated, then this path array contains only one constant value. - // This constant value needs to be duplicated N_max times for matrix conversion purposes. - ccu.adjustArrayLength(pm.pm_); - - ccu.copyMatrix(path_matrix, pm.pm_); - - return true; -} /* END TrajectoryProfileSinoid ******************************************************************************************/ From af73ea102f2be217e32a074363c7e127556e322e Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 15:34:45 +0100 Subject: [PATCH 27/35] get rid of ProfileTimings.ok --- .../cartesian_controller_data_types.h | 1 - .../trajectory_profile_generator_base.h | 7 +++---- .../trajectory_profile_generator_ramp.h | 2 +- .../trajectory_profile_generator_sinoid.h | 2 +- .../trajectory_profile_generator_ramp.cpp | 11 +++-------- .../trajectory_profile_generator_sinoid.cpp | 11 +++-------- 6 files changed, 11 insertions(+), 23 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 2c34e908..9ee2a47d 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -48,7 +48,6 @@ struct ProfileTimings { double tb, te, tv; unsigned int steps_tb, steps_te, steps_tv; - bool ok; double vel; }; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 8895f147..588e5316 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -60,7 +60,7 @@ class TrajectoryProfileBase cob_cartesian_controller::PathMatrix pm(lin, rot); // Get the profile timings from the longest path - pt_max_ = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true); + bool success = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true, pt_max_); // Calculate the paths for (unsigned int i = 0; i < pm.pm_.size(); i++) @@ -87,8 +87,7 @@ class TrajectoryProfileBase double vel_max = params_.profile.vel; // Calculate the Profile Timings - pt = getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false); - if (pt.ok) + if (getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false, pt)) { array = getTrajectory(pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } @@ -101,7 +100,7 @@ class TrajectoryProfileBase return true; } - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) = 0; + virtual bool getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) = 0; virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index 494ee3e9..d1d41343 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -43,7 +43,7 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase ~TrajectoryProfileRamp() {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); + virtual bool getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); }; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 78553079..b62266c8 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -43,7 +43,7 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase ~TrajectoryProfileSinoid() {} - virtual cob_cartesian_controller::ProfileTimings getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe); + virtual bool getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); }; diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index 49b534a2..de71b33b 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -31,10 +31,9 @@ #include /* BEGIN TrajectoryProfileRamp ********************************************************************************************/ -inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) +inline bool TrajectoryProfileRamp::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) { CartesianControllerUtils utils; - cob_cartesian_controller::ProfileTimings pt; double tv, tb = 0.0; // Calculate the Sinoid Profile Parameters @@ -62,14 +61,10 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileRamp::getProfil pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo; pt.vel = vel; - pt.ok = true; - } - else - { - pt.ok = false; + return true; } - return pt; + return false; } inline std::vector TrajectoryProfileRamp::getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index 1c3619c7..b74ac2ff 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -31,10 +31,9 @@ #include /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ -inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe) +inline bool TrajectoryProfileSinoid::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) { CartesianControllerUtils utils; - cob_cartesian_controller::ProfileTimings pt; double tv, tb = 0.0; // Calculate the Sinoid Profile Parameters @@ -62,14 +61,10 @@ inline cob_cartesian_controller::ProfileTimings TrajectoryProfileSinoid::getProf pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo; pt.vel = vel; - pt.ok = true; - } - else - { - pt.ok = false; + return true; } - return pt; + return false; } inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) From 43db5612d2d96bc9677e4f5a63f0428081afb089 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 15:41:10 +0100 Subject: [PATCH 28/35] re-order vel-acc --- .../trajectory_profile_generator_base.h | 10 +++++----- .../trajectory_profile_generator_ramp.h | 4 ++-- .../trajectory_profile_generator_sinoid.h | 4 ++-- .../trajectory_profile_generator_ramp.cpp | 4 ++-- .../trajectory_profile_generator_sinoid.cpp | 4 ++-- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 588e5316..9e07a304 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -60,7 +60,7 @@ class TrajectoryProfileBase cob_cartesian_controller::PathMatrix pm(lin, rot); // Get the profile timings from the longest path - bool success = getProfileTimings(pm.getMaxSe(), 0, params_.profile.accl, params_.profile.vel, true, pt_max_); + bool success = getProfileTimings(pm.getMaxSe(), 0, params_.profile.vel, params_.profile.accl, true, pt_max_); // Calculate the paths for (unsigned int i = 0; i < pm.pm_.size(); i++) @@ -87,9 +87,9 @@ class TrajectoryProfileBase double vel_max = params_.profile.vel; // Calculate the Profile Timings - if (getProfileTimings(pa.Se_, pt_max_.te, accl_max, vel_max, false, pt)) + if (getProfileTimings(pa.Se_, pt_max_.te, vel_max, accl_max, false, pt)) { - array = getTrajectory(pa.Se_, accl_max, pt.vel, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); + array = getTrajectory(pa.Se_, pt.vel, accl_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); } else { @@ -100,8 +100,8 @@ class TrajectoryProfileBase return true; } - virtual bool getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) = 0; - virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, + virtual bool getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) = 0; + virtual std::vector getTrajectory(double se, double vel, double accl, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; const cob_cartesian_controller::CartesianActionStruct& params_; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index d1d41343..a0ffce1b 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -43,8 +43,8 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase ~TrajectoryProfileRamp() {} - virtual bool getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); - virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, + virtual bool getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); + virtual std::vector getTrajectory(double se, double vel, double accl, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); }; /* END TrajectoryProfileRamp **********************************************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index b62266c8..808670a5 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -43,8 +43,8 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase ~TrajectoryProfileSinoid() {} - virtual bool getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); - virtual std::vector getTrajectory(double se, double accl, double vel, double t_ipo, + virtual bool getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); + virtual std::vector getTrajectory(double se, double vel, double accl, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); }; /* END TrajectoryProfileSinoid **********************************************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index de71b33b..40ff1ae7 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -31,7 +31,7 @@ #include /* BEGIN TrajectoryProfileRamp ********************************************************************************************/ -inline bool TrajectoryProfileRamp::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) +inline bool TrajectoryProfileRamp::getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) { CartesianControllerUtils utils; double tv, tb = 0.0; @@ -67,7 +67,7 @@ inline bool TrajectoryProfileRamp::getProfileTimings(double Se, double te, doubl return false; } -inline std::vector TrajectoryProfileRamp::getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) +inline std::vector TrajectoryProfileRamp::getTrajectory(double se, double vel, double accl, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) { std::vector array; unsigned int i = 1; diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index b74ac2ff..f39b65a5 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -31,7 +31,7 @@ #include /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ -inline bool TrajectoryProfileSinoid::getProfileTimings(double Se, double te, double accl, double vel, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) +inline bool TrajectoryProfileSinoid::getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) { CartesianControllerUtils utils; double tv, tb = 0.0; @@ -67,7 +67,7 @@ inline bool TrajectoryProfileSinoid::getProfileTimings(double Se, double te, dou return false; } -inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, double accl, double vel, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) +inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, double vel, double accl, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) { std::vector array; unsigned int i = 1; From c924a1805b37890842f9289dd4847655d241528b Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 16:14:49 +0100 Subject: [PATCH 29/35] significantly simplify function parameters for getTrajectory and getProfileTimings --- .../trajectory_profile_generator_base.h | 15 ++++++-------- .../trajectory_profile_generator_ramp.h | 5 ++--- .../trajectory_profile_generator_sinoid.h | 5 ++--- .../trajectory_profile_generator_ramp.cpp | 18 ++++++++++------- .../trajectory_profile_generator_sinoid.cpp | 20 +++++++++++-------- 5 files changed, 33 insertions(+), 30 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 9e07a304..62a1f305 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -60,7 +60,7 @@ class TrajectoryProfileBase cob_cartesian_controller::PathMatrix pm(lin, rot); // Get the profile timings from the longest path - bool success = getProfileTimings(pm.getMaxSe(), 0, params_.profile.vel, params_.profile.accl, true, pt_max_); + bool success = getProfileTimings(pm.getMaxSe(), 0, true, pt_max_); // Calculate the paths for (unsigned int i = 0; i < pm.pm_.size(); i++) @@ -83,13 +83,11 @@ class TrajectoryProfileBase { std::vector array; cob_cartesian_controller::ProfileTimings pt; - double accl_max = params_.profile.accl; - double vel_max = params_.profile.vel; // Calculate the Profile Timings - if (getProfileTimings(pa.Se_, pt_max_.te, vel_max, accl_max, false, pt)) + if (getProfileTimings(pa.Se_, pt_max_.te, false, pt)) { - array = getTrajectory(pa.Se_, pt.vel, accl_max, params_.profile.t_ipo, pt.steps_tb, pt.steps_tv, pt.steps_te, pt.tb, pt.tv, pt.te); + array = getTrajectory(pa.Se_, pt); } else { @@ -99,10 +97,9 @@ class TrajectoryProfileBase pa.array_ = array; return true; } - - virtual bool getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) = 0; - virtual std::vector getTrajectory(double se, double vel, double accl, double t_ipo, - double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) = 0; + + virtual bool getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) = 0; + virtual std::vector getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt) = 0; const cob_cartesian_controller::CartesianActionStruct& params_; cob_cartesian_controller::ProfileTimings pt_max_; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index a0ffce1b..e16498d7 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -43,9 +43,8 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase ~TrajectoryProfileRamp() {} - virtual bool getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); - virtual std::vector getTrajectory(double se, double vel, double accl, double t_ipo, - double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); + virtual bool getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); + virtual std::vector getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt); }; /* END TrajectoryProfileRamp **********************************************************************************************/ diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 808670a5..2ffc8d24 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -43,9 +43,8 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase ~TrajectoryProfileSinoid() {} - virtual bool getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); - virtual std::vector getTrajectory(double se, double vel, double accl, double t_ipo, - double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te); + virtual bool getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt); + virtual std::vector getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt); }; /* END TrajectoryProfileSinoid **********************************************************************************************/ diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp index 40ff1ae7..8fabb742 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp @@ -31,10 +31,12 @@ #include /* BEGIN TrajectoryProfileRamp ********************************************************************************************/ -inline bool TrajectoryProfileRamp::getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) +inline bool TrajectoryProfileRamp::getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) { CartesianControllerUtils utils; double tv, tb = 0.0; + double vel = params_.profile.vel; + double accl = params_.profile.accl; // Calculate the Sinoid Profile Parameters if (vel > sqrt(std::fabs(Se) * accl)) @@ -67,27 +69,29 @@ inline bool TrajectoryProfileRamp::getProfileTimings(double Se, double te, doubl return false; } -inline std::vector TrajectoryProfileRamp::getTrajectory(double se, double vel, double accl, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) +inline std::vector TrajectoryProfileRamp::getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt) { std::vector array; unsigned int i = 1; double direction = se/std::fabs(se); + double accl = params_.profile.accl; + double t_ipo = params_.profile.t_ipo; // Calculate the ramp profile path // 0 <= t <= tb - for (; i <= steps_tb; i++) + for (; i <= pt.steps_tb; i++) { array.push_back(direction * (0.5*accl*pow((t_ipo*i), 2))); } // tb <= t <= tv - for (; i <= (steps_tb + steps_tv); i++) + for (; i <= (pt.steps_tb + pt.steps_tv); i++) { - array.push_back(direction *(vel*(t_ipo*i) - 0.5*pow(vel, 2)/accl)); + array.push_back(direction *(pt.vel*(t_ipo*i) - 0.5*pow(pt.vel, 2)/accl)); } // tv <= t <= te - for (; i <= (steps_tb + steps_tv + steps_te + 1); i++) + for (; i <= (pt.steps_tb + pt.steps_tv + pt.steps_te + 1); i++) { - array.push_back(direction * (vel * tv - 0.5 * accl * pow(te-(t_ipo*i), 2))); + array.push_back(direction * (pt.vel * pt.tv - 0.5 * accl * pow(pt.te-(t_ipo*i), 2))); } return array; diff --git a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp index f39b65a5..58415b0b 100644 --- a/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp +++ b/cob_cartesian_controller/src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp @@ -31,10 +31,12 @@ #include /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/ -inline bool TrajectoryProfileSinoid::getProfileTimings(double Se, double te, double vel, double accl, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) +inline bool TrajectoryProfileSinoid::getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt) { CartesianControllerUtils utils; double tv, tb = 0.0; + double vel = params_.profile.vel; + double accl = params_.profile.accl; // Calculate the Sinoid Profile Parameters if (vel > sqrt(std::fabs(Se) * accl / 2)) @@ -67,27 +69,29 @@ inline bool TrajectoryProfileSinoid::getProfileTimings(double Se, double te, dou return false; } -inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, double vel, double accl, double t_ipo, double steps_tb, double steps_tv, double steps_te, double tb, double tv, double te) +inline std::vector TrajectoryProfileSinoid::getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt) { std::vector array; unsigned int i = 1; double direction = se/std::fabs(se); + double accl = params_.profile.accl; + double t_ipo = params_.profile.t_ipo; // Calculate the sinoid profile path // 0 <= t <= tb - for (; i <= steps_tb; i++) + for (; i <= pt.steps_tb; i++) { - array.push_back(direction * (accl*(0.25*pow(i*t_ipo, 2) + pow(tb, 2)/(8*pow(M_PI, 2)) *(cos(2*M_PI/tb * (i*t_ipo))-1)))); + array.push_back(direction * (accl*(0.25*pow(i*t_ipo, 2) + pow(pt.tb, 2)/(8*pow(M_PI, 2)) *(cos(2*M_PI/pt.tb * (i*t_ipo))-1)))); } // tb <= t <= tv - for (; i <= (steps_tb + steps_tv); i++) + for (; i <= (pt.steps_tb + pt.steps_tv); i++) { - array.push_back(direction * (vel*(i*t_ipo-0.5*tb))); + array.push_back(direction * (pt.vel*(i*t_ipo-0.5*pt.tb))); } // tv <= t <= te - for (; i <= (steps_tv + steps_tb + steps_te + 1); i++) + for (; i <= (pt.steps_tv + pt.steps_tb + pt.steps_te + 1); i++) { - array.push_back(direction * (0.5 * accl *(te*(i*t_ipo + tb) - 0.5*(pow(i*t_ipo, 2)+pow(te, 2)+2*pow(tb, 2)) + (pow(tb, 2)/(4*pow(M_PI, 2))) * (1-cos(((2*M_PI)/tb) * (i*t_ipo-tv)))))); + array.push_back(direction * (0.5 * accl *(pt.te*(i*t_ipo + pt.tb) - 0.5*(pow(i*t_ipo, 2)+pow(pt.te, 2)+2*pow(pt.tb, 2)) + (pow(pt.tb, 2)/(4*pow(M_PI, 2))) * (1-cos(((2*M_PI)/pt.tb) * (i*t_ipo-pt.tv)))))); } return array; From bc53c3394e3e993b49a2bb0426111f105c926708 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 16:20:37 +0100 Subject: [PATCH 30/35] remove unused Pose parameter --- .../trajectory_profile_generator_base.h | 3 +-- .../src/trajectory_interpolator/trajectory_interpolator.cpp | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h index 62a1f305..3309f7ec 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_base.h @@ -48,8 +48,7 @@ class TrajectoryProfileBase {} virtual bool calculateProfile(std::vector* path_matrix, - const double Se_lin, const double Se_rot, - geometry_msgs::Pose start) + const double Se_lin, const double Se_rot) { CartesianControllerUtils ccu; std::vector linear_path, angular_path; diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index 26582ed3..bb45e965 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -56,7 +56,7 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ double Se_rot = q_start.angleShortestPath(q_end); - if (!trajectory_profile_generator_->calculateProfile(path_matrix, Se_lin, Se_rot, as.move_lin.start)) + if (!trajectory_profile_generator_->calculateProfile(path_matrix, Se_lin, Se_rot)) { return false; } @@ -126,7 +126,7 @@ bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pos Se = std::fabs(Se); // Calculates the Path with RAMP or SINOID profile - if (!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0, pose)) + if (!this->trajectory_profile_generator_->calculateProfile(path_matrix, Se, 0)) { return false; } From 4a9d1960ec90442675fcfa0a1547a67b8f4b8c66 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 16:27:03 +0100 Subject: [PATCH 31/35] minor styling --- .../cartesian_controller_data_types.h | 6 +++--- .../trajectory_profile_generator_ramp.h | 2 +- .../trajectory_profile_generator_sinoid.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 9ee2a47d..2a7a3a18 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -74,9 +74,9 @@ struct CartesianActionStruct class PathArray { public: - PathArray(double Se, std::vector array): - Se_(Se), - array_(array) + PathArray(double Se, std::vector array) + : Se_(Se), + array_(array) {} ~PathArray() diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h index e16498d7..3f32977e 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h @@ -37,7 +37,7 @@ class TrajectoryProfileRamp: public TrajectoryProfileBase { public: explicit TrajectoryProfileRamp(const cob_cartesian_controller::CartesianActionStruct& params) - : TrajectoryProfileBase(params) + : TrajectoryProfileBase(params) {} ~TrajectoryProfileRamp() diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h index 2ffc8d24..0a7d82bf 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h @@ -37,7 +37,7 @@ class TrajectoryProfileSinoid : public TrajectoryProfileBase { public: explicit TrajectoryProfileSinoid(const cob_cartesian_controller::CartesianActionStruct& params) - : TrajectoryProfileBase(params) + : TrajectoryProfileBase(params) {} ~TrajectoryProfileSinoid() From ab97a19cfbbc6305b54d47c743f3bb50dfed85f2 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 16:27:44 +0100 Subject: [PATCH 32/35] remove movePTP --- .../cartesian_controller.h | 4 -- .../src/cartesian_controller.cpp | 46 ------------------- 2 files changed, 50 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h index b6c81a49..f343a35b 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h @@ -58,7 +58,6 @@ class CartesianController // Main functions bool posePathBroadcaster(const geometry_msgs::PoseArray& cartesian_path); - bool movePTP(const geometry_msgs::Pose& target_pose, double epsilon); // Helper function bool startTracking(); @@ -87,9 +86,6 @@ class CartesianController double update_rate_; std::string root_frame_, chain_tip_link_, target_frame_; - // HelperVars for movePTP - bool reached_pos_; - /// Action interface std::string action_name_; boost::shared_ptr as_; diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index efe9e71b..e67f060e 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -149,52 +149,6 @@ bool CartesianController::stopTracking() return success; } -// MovePTP -bool CartesianController::movePTP(const geometry_msgs::Pose& target_pose, const double epsilon) -{ - bool success = false; - int reached_pos_counter = 0; - - ros::Rate rate(update_rate_); - tf::StampedTransform stamped_transform; - stamped_transform.setOrigin(tf::Vector3(target_pose.position.x, - target_pose.position.y, - target_pose.position.z)); - stamped_transform.setRotation(tf::Quaternion(target_pose.orientation.x, - target_pose.orientation.y, - target_pose.orientation.z, - target_pose.orientation.w)); - stamped_transform.frame_id_ = root_frame_; - stamped_transform.child_frame_id_ = target_frame_; - - while (as_->isActive()) - { - // Send/Refresh target Frame - stamped_transform.stamp_ = ros::Time::now(); - tf_broadcaster_.sendTransform(stamped_transform); - - // Get transformation - tf::StampedTransform stamped_transform = utils_.getStampedTransform(target_frame_, chain_tip_link_); - - // Wait for chain_tip_link to be within epsilon area of target_frame - if (utils_.inEpsilonArea(stamped_transform, epsilon)) - { - reached_pos_counter++; // Count up if end effector position is in the epsilon area to avoid wrong values - } - - if (reached_pos_counter >= static_cast(2*update_rate_)) // has been close enough to target for 2 seconds - { - success = true; - break; - } - - rate.sleep(); - ros::spinOnce(); - } - - return success; -} - // Broadcasting interpolated Cartesian path bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& cartesian_path) { From d2ac2061f96f96a511ba48198dc9dba2f1a4e474 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Mon, 14 Dec 2015 16:41:01 +0100 Subject: [PATCH 33/35] use const in function parameters --- .../cartesian_controller_data_types.h | 6 +++--- .../cob_cartesian_controller/cartesian_controller_utils.h | 5 ++--- .../trajectory_interpolator/trajectory_interpolator.h | 4 ++-- cob_cartesian_controller/src/cartesian_controller_utils.cpp | 4 ++-- .../src/trajectory_interpolator/trajectory_interpolator.cpp | 4 ++-- 5 files changed, 11 insertions(+), 12 deletions(-) diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h index 2a7a3a18..456b8b7b 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h @@ -74,7 +74,7 @@ struct CartesianActionStruct class PathArray { public: - PathArray(double Se, std::vector array) + PathArray(const double Se, const std::vector array) : Se_(Se), array_(array) {} @@ -91,8 +91,8 @@ class PathArray class PathMatrix { public: - PathMatrix(PathArray& pa1, - PathArray& pa2) + PathMatrix(const PathArray pa1, + const PathArray pa2) { pm_.push_back(pa1); pm_.push_back(pa2); diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h index 37756849..a318730c 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h @@ -55,12 +55,11 @@ class CartesianControllerUtils bool inEpsilonArea(const tf::StampedTransform& stamped_transform, const double epsilon); void poseToRPY(const geometry_msgs::Pose& pose, double& roll, double& pitch, double& yaw); - void previewPath(const geometry_msgs::PoseArray& pose_array); + void previewPath(const geometry_msgs::PoseArray pose_array); - void sortMatrixByIdx(std::vector& m); void adjustArrayLength(std::vector& m); void copyMatrix(std::vector* path_array, std::vector& m); - double roundUpToMultiplier(double numberToRound, double multiplier); + double roundUpToMultiplier(const double numberToRound, const double multiplier); private: ros::NodeHandle nh_; diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h index 3417a1a7..f9360e60 100644 --- a/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h +++ b/cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h @@ -50,10 +50,10 @@ class TrajectoryInterpolator } bool linearInterpolation(geometry_msgs::PoseArray& pose_array, - const cob_cartesian_controller::CartesianActionStruct& as); + const cob_cartesian_controller::CartesianActionStruct as); bool circularInterpolation(geometry_msgs::PoseArray& pose_array, - const cob_cartesian_controller::CartesianActionStruct& as); + const cob_cartesian_controller::CartesianActionStruct as); private: std::string root_frame_; diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index 7dc1642a..bd2d4423 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -130,7 +130,7 @@ void CartesianControllerUtils::poseToRPY(const geometry_msgs::Pose& pose, double tf::Matrix3x3(q).getRPY(roll, pitch, yaw); } -void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray& pose_array) +void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray pose_array) { visualization_msgs::Marker marker; marker.type = visualization_msgs::Marker::SPHERE; @@ -183,7 +183,7 @@ void CartesianControllerUtils::copyMatrix(std::vector* path_array, std:: } } -double CartesianControllerUtils::roundUpToMultiplier(double numberToRound, double multiplier) +double CartesianControllerUtils::roundUpToMultiplier(const double numberToRound, const double multiplier) { return ( multiplier - std::fmod(numberToRound, multiplier) + numberToRound ); } diff --git a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp index bb45e965..6d8faf01 100644 --- a/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp +++ b/cob_cartesian_controller/src/trajectory_interpolator/trajectory_interpolator.cpp @@ -33,7 +33,7 @@ #include bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_array, - const cob_cartesian_controller::CartesianActionStruct& as) + const cob_cartesian_controller::CartesianActionStruct as) { this->trajectory_profile_generator_.reset(TrajectoryProfileBuilder::createProfile(as)); @@ -97,7 +97,7 @@ bool TrajectoryInterpolator::linearInterpolation(geometry_msgs::PoseArray& pose_ } bool TrajectoryInterpolator::circularInterpolation(geometry_msgs::PoseArray& pose_array, - const cob_cartesian_controller::CartesianActionStruct& as) + const cob_cartesian_controller::CartesianActionStruct as) { pose_array.header.stamp = ros::Time::now(); pose_array.header.frame_id = root_frame_; From 8c47ddbbb119f8a6735417b7c1caed31f2db8849 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Tue, 15 Dec 2015 17:03:26 +0100 Subject: [PATCH 34/35] use ros::Time::now --- .../src/cartesian_controller_utils.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index bd2d4423..9f47e8f1 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -53,8 +53,8 @@ tf::StampedTransform CartesianControllerUtils::getStampedTransform(const std::st { try { - ros::Time now = ros::Time(0); - tf_listener_.waitForTransform(target_frame, source_frame, now, ros::Duration(0.5)); + ros::Time now = ros::Time::now(); + tf_listener_.waitForTransform(target_frame, source_frame, now, ros::Duration(0.1)); tf_listener_.lookupTransform(target_frame, source_frame, now, stamped_transform); transform = true; } @@ -79,8 +79,8 @@ void CartesianControllerUtils::transformPose(const std::string source_frame, con { try { - stamped_in.header.stamp = ros::Time(0); - tf_listener_.waitForTransform(target_frame, source_frame, stamped_in.header.stamp, ros::Duration(1.0)); + ros::Time now = ros::Time::now(); + tf_listener_.waitForTransform(target_frame, source_frame, now, ros::Duration(0.1)); tf_listener_.transformPose(target_frame, stamped_in, stamped_out); pose_out = stamped_out.pose; transform = true; From 1a29538b0de45b34cfb870b5d9f438b6cddb5043 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Tue, 15 Dec 2015 17:08:27 +0100 Subject: [PATCH 35/35] remove lwa4d test scripts --- .../scripts/lwa4d/lwa4d_circle.py | 136 ----------------- .../scripts/lwa4d/lwa4d_quad.py | 139 ------------------ .../scripts/lwa4d/lwa4d_quad_angular.py | 121 --------------- 3 files changed, 396 deletions(-) delete mode 100755 cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py delete mode 100755 cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py delete mode 100755 cob_cartesian_controller/scripts/lwa4d/lwa4d_quad_angular.py diff --git a/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py b/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py deleted file mode 100755 index e8fbb9af..00000000 --- a/cob_cartesian_controller/scripts/lwa4d/lwa4d_circle.py +++ /dev/null @@ -1,136 +0,0 @@ -#! /usr/bin/env python -import math -import rospy - -from geometry_msgs.msg import Pose -from cob_cartesian_controller.msg import Profile -from simple_script_server.simple_script_server import simple_script_server -import simple_cartesian_interface as sci - - -def init_pos(): - sss = simple_script_server() - sss.move("arm", [[-0.0002934322105607734, -0.38304632633953606, 6.931483707006691e-07, 0.8526320037121202, 5.69952198326007e-07, -0.47039657856235184, -0.00029228225570943067]]) - -if __name__ == '__main__': - rospy.init_node('test_move_circ_interface') - init_pos() - - - pose = sci.gen_pose(pos=[-0.2, 0, 0.8], rpy=[0, 0.0, 0]) - profile = Profile() - profile.vel = 0.2 - profile.accl = 0.1 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - - - pose = sci.gen_pose(pos=[0.0, 0.0, 0.8], rpy=[0.0, 0.0, 0]) - start_angle = 180.0 * math.pi / 180.0 - end_angle = 0.0 * math.pi / 180.0 - profile = Profile() - profile.vel = 0.4 - profile.accl = 0.3 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_circ(pose, "world", start_angle, end_angle, 0.2, profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - - pose = sci.gen_pose(pos=[0.2, 0.0, 0.8], rpy=[0, math.pi/2, 0]) - profile = Profile() - profile.vel = 0.3 - profile.accl = 0.3 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - - - pose = sci.gen_pose(pos=[0.0, -0.2, 0.8], rpy=[0, 0.0, math.pi/2]) - profile = Profile() - profile.vel = 0.3 - profile.accl = 0.3 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - - - - - pose = sci.gen_pose(pos=[0.0, 0.0, 0.8], rpy=[0.0, 0.0, math.pi/2]) - start_angle = 180.0 * math.pi / 180.0 - end_angle = 0.0 * math.pi / 180.0 - profile = Profile() - profile.vel = 0.4 - profile.accl = 0.3 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_circ(pose, "world", start_angle, end_angle, 0.2, profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - - - - pose = sci.gen_pose(pos=[0.0, 0.2, 0.8], rpy=[0, math.pi/2, 0]) - profile = Profile() - profile.vel = 0.3 - profile.accl = 0.3 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - - - - pose = sci.gen_pose(pos=[-0.2, 0.0, 0.8], rpy=[0, 0, 0]) - profile = Profile() - profile.vel = 0.3 - profile.accl = 0.3 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - init_pos() - - diff --git a/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py b/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py deleted file mode 100755 index 027d9dcb..00000000 --- a/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad.py +++ /dev/null @@ -1,139 +0,0 @@ -#! /usr/bin/env python -import math -import rospy - -from geometry_msgs.msg import Pose -from cob_cartesian_controller.msg import Profile -from simple_script_server.simple_script_server import simple_script_server -import simple_cartesian_interface as sci - - -def init_pos(): - sss = simple_script_server() - sss.move("arm", [[-0.00032004093963244884, -0.7064191894021441, -1.577532922958369e-06, 1.4183686971944311, 1.2084352562169443e-05, -0.6913530502577565, -0.0002663056533762642]]) - -if __name__ == '__main__': - rospy.init_node('test_move_lin_interface') - init_pos() - - - pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0.0, 0.0, 0.0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - - rospy.sleep(3.0) - - pose = sci.gen_pose(pos=[0.3, 0, 0.9], rpy=[0.0, 0.0, math.pi/3]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - - rospy.sleep(1.0) - - pose = sci.gen_pose(pos=[0.3, 0, 0.8], rpy=[0.0, -0.2, 0.5]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - - rospy.sleep(1.0) - - - pose = sci.gen_pose(pos=[-0.3, 0, 0.8], rpy=[0.0, 1.0 , -0.5]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - - rospy.sleep(1.0) - - pose = sci.gen_pose(pos=[-0.3, 0, 0.9], rpy=[0.0, 0.0, 0.0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(1.0) - - - - pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0.0, 0.0, 0.0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(1.0) - - - - pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0.5, 0.0, 0.0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - profile.profile_type = Profile.SINOID - #profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(1.0) diff --git a/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad_angular.py b/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad_angular.py deleted file mode 100755 index 9db51481..00000000 --- a/cob_cartesian_controller/scripts/lwa4d/lwa4d_quad_angular.py +++ /dev/null @@ -1,121 +0,0 @@ -#! /usr/bin/env python -import math -import rospy - -from geometry_msgs.msg import Pose -from cob_cartesian_controller.msg import Profile -from simple_script_server.simple_script_server import simple_script_server -import simple_cartesian_interface as sci - - -def init_pos(): - sss = simple_script_server() - sss.move("arm", [[-0.00032004093963244884, -0.7064191894021441, -1.577532922958369e-06, 1.4183686971944311, 1.2084352562169443e-05, -0.6913530502577565, -0.0002663056533762642]]) - -if __name__ == '__main__': - rospy.init_node('test_move_lin_interface') - init_pos() - - - pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0, 0.0, 0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(3.0) - - - pose = sci.gen_pose(pos=[0.3, 0, 0.9], rpy=[0, 0.0, 0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(3.0) - - - pose = sci.gen_pose(pos=[0.3, 0, 0.8], rpy=[0, 0.0, 0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(3.0) - - - - pose = sci.gen_pose(pos=[-0.3, 0, 0.8], rpy=[0, 0.0, 0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(3.0) - - - pose = sci.gen_pose(pos=[-0.3, 0, 0.9], rpy=[0, 0.0, 0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(2.0) - - - - pose = sci.gen_pose(pos=[0.0, 0, 0.9], rpy=[0, 0.0, 0]) - profile = Profile() - profile.vel = 0.1 - profile.accl = 0.05 - #profile.profile_type = Profile.SINOID - profile.profile_type = Profile.RAMP - - success, message = sci.move_lin(pose, "world", profile) - if success: - rospy.loginfo(message) - else: - rospy.logerr(message) - - #init_pos() - rospy.sleep(3.0)