Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Joint limits debug helper functionality #215

Open
wants to merge 2 commits into
base: indigo-devel
Choose a base branch
from
Open

Joint limits debug helper functionality #215

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
41f9223
Debugging helper functionality
davetcoleman Nov 3, 2015
7fe4bc3
TEMP: enable joint limit debugging
davetcoleman Nov 11, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions joint_limits_interface/include/joint_limits_interface/joint_limits.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,26 @@ struct JointLimits
angle_wraparound(false)
{}

public:

// Helper function for debuggging, not realtime safe
void print()
{
std::cout << "min_position " << min_position << std::endl;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

pedantic: I'd only use std::endl in the last line, and use "\n" instead for all the others, to avoid flushing for each line: http://en.cppreference.com/w/cpp/io/manip/endl

std::cout << "max_position " << max_position << std::endl;
std::cout << "max_velocity " << max_velocity << std::endl;
std::cout << "max_acceleration " << max_acceleration << std::endl;
std::cout << "max_jerk " << max_jerk << std::endl;
std::cout << "max_effort " << max_effort << std::endl;

std::cout << "has_position_limits " << has_position_limits << std::endl;
std::cout << "has_velocity_limits " << has_velocity_limits << std::endl;
std::cout << "has_acceleration_limits " << has_acceleration_limits << std::endl;
std::cout << "has_jerk_limits " << has_jerk_limits << std::endl;
std::cout << "has_effort_limits " << has_effort_limits << std::endl;
std::cout << "angle_wraparound " << angle_wraparound << std::endl;
}

double min_position;
double max_position;
double max_velocity;
Expand Down
21 changes: 21 additions & 0 deletions joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,27 @@ class PositionJointSaturationHandle
}

const double cmd = internal::saturate(jh_.getCommand(), min_pos, max_pos);

// Optional helper code for debugging, not realtime safe ----------
#define USE_JOINT_LIMIT_DEBUG // Show warnings when joint limits exceeded
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have the impression that this #define should be defined in the CMakeLists.txt as a cmake option (https://cmake.org/cmake/help/v3.0/command/option.html):

option(BUILD_WITH_JOINT_LIMIT_DEBUG "Build with Joint Limit Debugging mode" OFF)
if(BUILD_WITH_JOINT_LIMIT_DEBUG)
  add_definitions(-DUSE_JOINT_LIMIT_DEBUG)
endif(BUILD_WITH_JOINT_LIMIT_DEBUG)

#ifdef USE_JOINT_LIMIT_DEBUG
if (cmd != jh_.getCommand())
{
// Determine if velocity or position is what limited us
if (min_pos != min_pos_limit_ || max_pos != max_pos_limit_)
std::cout << "VELOCITY LIMIT ";
else
std::cout << "POSITION LIMIT ";

// Limit violation details
std::cout << jh_.getName() << " position - original: " << jh_.getCommand() << " new: " << cmd
<< " diff: " << jh_.getCommand() - cmd
<< " vel_pos_delta: " << (limits_.max_velocity * period.toSec())
<< std::endl;
}
#endif
// ----------------------------------------------------

jh_.setCommand(cmd);
prev_cmd_ = cmd;
}
Expand Down