Skip to content

Commit

Permalink
Merge pull request #87 from ipa-fxm/feature_enforce_position_limits
Browse files Browse the repository at this point in the history 
Feature enforce position limits
  • Loading branch information
@ipa-nhg
ipa-nhg committed Feb 19, 2016
2 parents 1d8c6fb + 24a2bed commit 9ae7ce6
Show file tree
Hide file tree
Showing 20 changed files with 595 additions and 119 deletions.
1 change: 1 addition & 0 deletions cob_frame_tracker/cfg/FrameTracker.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("enable_abortion_checking", bool_t, 0, "Enable abortion checks and publish_hold_twist", True)
gen.add("cart_min_dist_threshold_lin", double_t, 0, "Cartesian minimal distance for tracking lin", 0.2, 0.0, 0.5)
gen.add("cart_min_dist_threshold_rot", double_t, 0, "Cartesian minimal distance for tracking rot", 0.2, 0.0, 0.5)
gen.add("twist_dead_threshold_lin", double_t, 0, "Twist dead threshold lin", 0.05, 0.0, 0.5)
Expand Down
1 change: 1 addition & 0 deletions cob_frame_tracker/include/cob_frame_tracker/cob_frame_tracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,6 +176,7 @@ class CobFrameTracker
double tracking_duration_;
ros::Time tracking_start_time_;

bool enable_abortion_checking_;
double cart_min_dist_threshold_lin_;
double cart_min_dist_threshold_rot_;
double twist_dead_threshold_lin_;
Expand Down
2 changes: 1 addition & 1 deletion cob_frame_tracker/include/cob_frame_tracker/interactive_frame_target.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ class InteractiveFrameTarget
ros::ServiceClient start_lookat_client_;
ros::ServiceClient stop_client_;

void updateMarker();
void updateMarker(const std::string& frame);
void sendTransform(const ros::TimerEvent& event);
void startTracking( const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback );
void startLookat( const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback );
Expand Down
136 changes: 93 additions & 43 deletions cob_frame_tracker/src/cob_frame_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ bool CobFrameTracker::initialize()
if (nh_tracker.hasParam("update_rate"))
{ nh_tracker.getParam("update_rate", update_rate_); }
else
{ update_rate_ = 68.0; } //hz
{ update_rate_ = 50.0; } //hz

if (nh_.hasParam("chain_base_link"))
{
Expand Down Expand Up @@ -109,12 +109,12 @@ bool CobFrameTracker::initialize()
if (nh_tracker.hasParam("max_vel_lin"))
{ nh_tracker.getParam("max_vel_lin", max_vel_lin_); }
else
{ max_vel_lin_ = 10.0; } //m/sec
{ max_vel_lin_ = 0.1; } //m/sec

if (nh_tracker.hasParam("max_vel_rot"))
{ nh_tracker.getParam("max_vel_rot", max_vel_rot_); }
else
{ max_vel_rot_ = 6.28; } //rad/sec
{ max_vel_rot_ = 0.1; } //rad/sec

// Load PID Controller using gains set on parameter server
pid_controller_trans_x_.init(ros::NodeHandle(nh_tracker, "pid_trans_x"));
Expand All @@ -139,6 +139,7 @@ bool CobFrameTracker::initialize()
lookat_focus_frame_ = "lookat_focus_frame";

//ABORTION CRITERIA:
enable_abortion_checking_ = true;
cart_min_dist_threshold_lin_ = 0.01;
cart_min_dist_threshold_rot_ = 0.01;
twist_dead_threshold_lin_ = 0.05;
Expand Down Expand Up @@ -375,16 +376,27 @@ bool CobFrameTracker::startTrackingCallback(cob_srvs::SetString::Request& reques
}
else
{
std::string msg = "CobFrameTracker: StartTracking started with CART_DIST_SECURITY MONITORING enabled";
ROS_INFO_STREAM(msg);
response.success = true;
response.message = msg;

tracking_ = true;
tracking_goal_ = false;
lookat_ = false;
tracking_frame_ = chain_tip_link_;
target_frame_ = request.data;
// check whether given target frame exists
if(!tf_listener_.frameExists(request.data))
{
std::string msg = "CobFrameTracker: StartTracking denied because target frame '" + request.data + "' does not exist";
ROS_ERROR_STREAM(msg);
response.success = false;
response.message = msg;
}
else
{
std::string msg = "CobFrameTracker: StartTracking started with CART_DIST_SECURITY MONITORING enabled";
ROS_INFO_STREAM(msg);
response.success = true;
response.message = msg;

tracking_ = true;
tracking_goal_ = false;
lookat_ = false;
tracking_frame_ = chain_tip_link_;
target_frame_ = request.data;
}
}
return true;
}
Expand Down Expand Up @@ -414,33 +426,44 @@ bool CobFrameTracker::startLookatCallback(cob_srvs::SetString::Request& request,
}
else
{
dynamic_reconfigure::Reconfigure srv;
dynamic_reconfigure::IntParameter int_param;
int_param.name = "kinematic_extension";
int_param.value = 4; //LOOKAT
srv.request.config.ints.push_back(int_param);

bool success = reconfigure_client_.call(srv);

if(success)
// check whether given target frame exists
if(!tf_listener_.frameExists(request.data))
{
std::string msg = "CobFrameTracker: StartLookat started with CART_DIST_SECURITY MONITORING enabled";
ROS_INFO_STREAM(msg);
response.success = true;
std::string msg = "CobFrameTracker: StartLookat denied because target frame '" + request.data + "' does not exist";
ROS_ERROR_STREAM(msg);
response.success = false;
response.message = msg;

tracking_ = false;
tracking_goal_ = false;
lookat_ = true;
tracking_frame_ = lookat_focus_frame_;
target_frame_ = request.data;
}
else
{
std::string msg = "CobFrameTracker: StartLookat denied because DynamicReconfigure failed";
ROS_ERROR_STREAM(msg);
response.success = false;
response.message = msg;
dynamic_reconfigure::Reconfigure srv;
dynamic_reconfigure::IntParameter int_param;
int_param.name = "kinematic_extension";
int_param.value = 4; //LOOKAT
srv.request.config.ints.push_back(int_param);

bool success = reconfigure_client_.call(srv);

if(success)
{
std::string msg = "CobFrameTracker: StartLookat started with CART_DIST_SECURITY MONITORING enabled";
ROS_INFO_STREAM(msg);
response.success = true;
response.message = msg;

tracking_ = false;
tracking_goal_ = false;
lookat_ = true;
tracking_frame_ = lookat_focus_frame_;
target_frame_ = request.data;
}
else
{
std::string msg = "CobFrameTracker: StartLookat denied because DynamicReconfigure failed";
ROS_ERROR_STREAM(msg);
response.success = false;
response.message = msg;
}
}
}
return true;
Expand Down Expand Up @@ -502,14 +525,28 @@ void CobFrameTracker::goalCB()
if (as_->isNewGoalAvailable())
{
boost::shared_ptr<const cob_frame_tracker::FrameTrackingGoal> goal_= as_->acceptNewGoal();
target_frame_ = goal_->tracking_frame;
tracking_duration_ = goal_->tracking_duration;
stop_on_goal_ = goal_->stop_on_goal;
tracking_ = false;
tracking_goal_ = true;
lookat_ = false;
abortion_counter_ = 0;
tracking_start_time_ = ros::Time::now();

if(tracking_ || lookat_)
{
// Goal should not be accepted
ROS_ERROR_STREAM("CobFrameTracker: Received ActionGoal while tracking/lookat Service is active!");
}
else if (!tf_listener_.frameExists(goal_->tracking_frame))
{
// Goal should not be accepted
ROS_ERROR_STREAM("CobFrameTracker: Received ActionGoal but target frame '" << goal_->tracking_frame << "' does not exist");
}
else
{
target_frame_ = goal_->tracking_frame;
tracking_duration_ = goal_->tracking_duration;
stop_on_goal_ = goal_->stop_on_goal;
tracking_ = false;
tracking_goal_ = true;
lookat_ = false;
abortion_counter_ = 0;
tracking_start_time_ = ros::Time::now();
}
}
}

Expand Down Expand Up @@ -560,6 +597,12 @@ int CobFrameTracker::checkStatus()
{
int status = 0;

if(!enable_abortion_checking_)
{
abortion_counter_ = 0;
return status;
}

if(ros::Time::now() > tracking_start_time_ + ros::Duration(tracking_duration_))
{
action_result_.success = true;
Expand Down Expand Up @@ -603,6 +646,12 @@ int CobFrameTracker::checkServiceCallStatus()
{
int status = 0;

if(!enable_abortion_checking_)
{
abortion_counter_ = 0;
return status;
}

bool distance_violation = checkCartDistanceViolation(cart_distance_, 0.0);

if(distance_violation)
Expand Down Expand Up @@ -666,6 +715,7 @@ void CobFrameTracker::jointstateCallback(const sensor_msgs::JointState::ConstPtr

void CobFrameTracker::reconfigureCallback(cob_frame_tracker::FrameTrackerConfig& config, uint32_t level)
{
enable_abortion_checking_ = config.enable_abortion_checking;
cart_min_dist_threshold_lin_ = config.cart_min_dist_threshold_lin;
cart_min_dist_threshold_rot_ = config.cart_min_dist_threshold_rot;
twist_dead_threshold_lin_ = config.twist_dead_threshold_lin;
Expand Down
17 changes: 11 additions & 6 deletions cob_frame_tracker/src/interactive_frame_target.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ bool InteractiveFrameTarget::initialize()
if (nh_tracker.hasParam("update_rate"))
{ nh_tracker.getParam("update_rate", update_rate_); }
else
{ update_rate_ = 68.0; } //hz
{ update_rate_ = 50.0; } //hz

if (nh_.hasParam("chain_tip_link"))
{
Expand Down Expand Up @@ -232,23 +232,25 @@ bool InteractiveFrameTarget::initialize()
void InteractiveFrameTarget::sendTransform(const ros::TimerEvent& event)
{
if(!tracking_ && !lookat_)
{ updateMarker(); }
{ updateMarker(tracking_frame_); }

target_pose_.stamp_ = ros::Time::now();
target_pose_.child_frame_id_ = target_frame_;
tf_broadcaster_.sendTransform(target_pose_);
}

void InteractiveFrameTarget::updateMarker()
void InteractiveFrameTarget::updateMarker(const std::string& frame)
{
bool transform_available = false;
while(!transform_available)
{
try{
tf_listener_.lookupTransform(root_frame_, tracking_frame_, ros::Time(), target_pose_);
try
{
tf_listener_.lookupTransform(root_frame_, frame, ros::Time(), target_pose_);
transform_available = true;
}
catch (tf::TransformException& ex){
catch (tf::TransformException& ex)
{
//ROS_WARN("IFT::updateMarker: Waiting for transform...(%s)",ex.what());
ros::Duration(0.1).sleep();
}
Expand Down Expand Up @@ -293,6 +295,9 @@ void InteractiveFrameTarget::startLookat( const visualization_msgs::InteractiveM
if(success && srv.response.success)
{
ROS_INFO_STREAM("StartLookat successful: " << srv.response.message);

updateMarker("lookat_focus_frame");

tracking_ = false;
lookat_ = true;
}
Expand Down
19 changes: 17 additions & 2 deletions cob_twist_controller/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ add_executable(cob_twist_controller_node src/cob_twist_controller_node.cpp)
add_dependencies(cob_twist_controller_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(cob_twist_controller_node twist_controller ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})


### DEBUG NODES ###
add_executable(debug_trajectory_marker_node src/debug/debug_trajectory_marker_node.cpp)
add_dependencies(debug_trajectory_marker_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(debug_trajectory_marker_node ${catkin_LIBRARIES})
Expand All @@ -70,7 +72,6 @@ add_executable(debug_evaluate_jointstates_node src/debug/debug_evaluate_jointsta
add_dependencies(debug_evaluate_jointstates_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(debug_evaluate_jointstates_node ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})


add_executable(test_moving_average_node src/debug/test_moving_average_node.cpp)
add_dependencies(test_moving_average_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_moving_average_node ${catkin_LIBRARIES})
Expand All @@ -79,10 +80,24 @@ add_executable(test_simpson_integrator_node src/debug/test_simpson_integrator_no
add_dependencies(test_simpson_integrator_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_simpson_integrator_node ${catkin_LIBRARIES})

add_executable(test_trajectory_command_sine_node src/debug/test_trajectory_command_sine_node.cpp)
add_dependencies(test_trajectory_command_sine_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_trajectory_command_sine_node ${catkin_LIBRARIES})

add_executable(test_forward_command_sine_node src/debug/test_forward_command_sine_node.cpp)
add_dependencies(test_forward_command_sine_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_forward_command_sine_node ${catkin_LIBRARIES})

roslint_cpp()

### INSTALL ###
install(TARGETS cob_twist_controller_node damping_methods inv_calculations constraint_solvers limiters controller_interfaces kinematic_extensions inverse_differential_kinematics_solver twist_controller debug_trajectory_marker_node debug_evaluate_jointstates_node
install(TARGETS cob_twist_controller_node damping_methods inv_calculations constraint_solvers limiters controller_interfaces kinematic_extensions inverse_differential_kinematics_solver twist_controller
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

install(TARGETS debug_trajectory_marker_node debug_evaluate_jointstates_node test_moving_average_node test_simpson_integrator_node test_trajectory_command_sine_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
Expand Down
7 changes: 4 additions & 3 deletions cob_twist_controller/cfg/TwistController.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ kinematic_extension_enum = gen.enum([

# ==================================== Controller interfaces =================================================================================
ctrl_interface = gen.add_group("Controller Interfaces", "ctrl_interface")
ctrl_interface.add("controller_interface", int_t, 0, "The controller interface to use", 0, None, None, edit_method=controller_interface_enum)
ctrl_interface.add("controller_interface", int_t, 0, "The controller interface to use", 0, None, None, edit_method=controller_interface_enum)
ctrl_interface.add("integrator_smoothing", double_t, 0, "The factor used for exponential smoothing during simpson integration)", 0.2, 0, 1)

# ==================================== Damping and truncation (singular value adaption) ====================================================
damp_trunc = gen.add_group("Damping and Truncation", "damping_truncation")
Expand Down Expand Up @@ -96,13 +97,13 @@ limits.add("keep_direction", bool_t, 0, "With keep_direction the whole joi
limits.add("enforce_pos_limits", bool_t, 0, "If 'True', enforce joint position limits in TwistController, else let RobotHW handle the limits", True)
limits.add("enforce_vel_limits", bool_t, 0, "If 'True', enforce joint velocity limits in TwistController, else let RobotHW handle the limits", True)
limits.add("enforce_acc_limits", bool_t, 0, "If 'True', enforce joint acceleration limits in TwistController, else let RobotHW handle the limits", False)
limits.add("limits_tolerance", double_t, 0, "Tolerance around joint position limits where velocities are normalized [degrees]. Must not be zero else DIV/0!", 5, 0.0, 15)
limits.add("limits_tolerance", double_t, 0, "Tolerance around joint position limits where velocities are normalized [degrees]. Must not be zero else DIV/0!", 30, 0.0, 90)
limits.add("max_vel_lin_base", double_t, 0, "Maximum linear velocity to be commanded to the base in [m/s].", 0.5, 0, 1.5)
limits.add("max_vel_rot_base", double_t, 0, "Maximum rotational velocity to be commanded to the base in [rad/s].", 0.5, 0, 1.5)

# ==================================== Parameters for kinematics extensions =====================================================
kin_ext = gen.add_group("Kinematics Extension", "kin_ext")
kin_ext.add("kinematic_extension", int_t, 0, "Consider additional DoF", 0, None, None, edit_method=kinematic_extension_enum)
kin_ext.add("extension_ratio", double_t, 0, "Value for ratio between chain and extension", 0.01, 0.0, 1.0)
kin_ext.add("extension_ratio", double_t, 0, "Value for ratio between chain and extension", 0.01, 0.0, 1.0)

exit(gen.generate(PACKAGE, "cob_twist_controller", "TwistController"))
Loading

0 comments on commit 9ae7ce6

Please sign in to comment.