Skip to content

Commit

Permalink
Using joint limits structure for the admittance controller.
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl committed Sep 7, 2021
1 parent bb0c9e6 commit b829d81
Show file tree
Hide file tree
Showing 2 changed files with 74 additions and 23 deletions.
32 changes: 20 additions & 12 deletions ...e/joint_limits_interface/joint_limits.hpp → ...ace/include/joint_limits/joint_limits.hpp
View file
Expand Up @@ -14,20 +14,22 @@

/// \author Adolfo Rodriguez Tsouroukdissian

#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_
#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_
#ifndef JOINT_LIMITS__JOINT_LIMITS_HPP_
#define JOINT_LIMITS__JOINT_LIMITS_HPP_

namespace joint_limits_interface
#include <limits>

namespace joint_limits
{
struct JointLimits
{
JointLimits()
: min_position(0.0),
max_position(0.0),
max_velocity(0.0),
max_acceleration(0.0),
max_jerk(0.0),
max_effort(0.0),
: min_position(std::numeric_limits<double>::quiet_NaN()),
max_position(std::numeric_limits<double>::quiet_NaN()),
max_velocity(std::numeric_limits<double>::quiet_NaN()),
max_acceleration(std::numeric_limits<double>::quiet_NaN()),
max_jerk(std::numeric_limits<double>::quiet_NaN()),
max_effort(std::numeric_limits<double>::quiet_NaN()),
has_position_limits(false),
has_velocity_limits(false),
has_acceleration_limits(false),
Expand All @@ -54,14 +56,20 @@ struct JointLimits

struct SoftJointLimits
{
SoftJointLimits() : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0) {}
SoftJointLimits()
: min_position(std::numeric_limits<double>::quiet_NaN()),
max_position(std::numeric_limits<double>::quiet_NaN()),
k_position(std::numeric_limits<double>::quiet_NaN()),
k_velocity(std::numeric_limits<double>::quiet_NaN())
{
}

double min_position;
double max_position;
double k_position;
double k_velocity;
};

} // namespace joint_limits_interface
} // namespace joint_limits

#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_
#endif // JOINT_LIMITS__JOINT_LIMITS_HPP_
65 changes: 54 additions & 11 deletions ...imits_interface/joint_limits_rosparam.hpp → ...de/joint_limits/joint_limits_rosparam.hpp
View file
Expand Up @@ -14,17 +14,60 @@

/// \author Adolfo Rodriguez Tsouroukdissian

#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_
#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_

#include <joint_limits_interface/joint_limits.hpp>

#include <rclcpp/rclcpp.hpp>
#ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_

#include <limits>
#include <string>

namespace joint_limits_interface
#include "joint_limits/joint_limits.hpp"
#include "rclcpp/rclcpp.hpp"

namespace joint_limits
{
inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node)
{
const std::string param_base_name = "joint_limits." + joint_name;
try
{
node->declare_parameter<bool>(param_base_name + ".has_position_limits", false);
node->declare_parameter<double>(
param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(
param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".has_velocity_limits", false);
node->declare_parameter<double>(
param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(
param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".has_acceleration_limits", false);
node->declare_parameter<double>(
param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".has_jerk_limits", false);
node->declare_parameter<double>(
param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".has_effort_limits", false);
node->declare_parameter<double>(
param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<bool>(param_base_name + ".angle_wraparound", false);
node->declare_parameter<bool>(param_base_name + ".has_soft_limits", false);
node->declare_parameter<double>(
param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(
param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(
param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
node->declare_parameter<double>(
param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
}
catch (const std::exception & ex)
{
RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
return false;
}
return true;
}

/// Populate a JointLimits instance from the ROS parameter server.
/**
* It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters
Expand Down Expand Up @@ -58,7 +101,7 @@ namespace joint_limits_interface
* existing values. Values in \p limits not specified in the parameter server remain unchanged.
* \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), false otherwise.
*/
inline bool getJointLimits(
inline bool get_joint_limits(
const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits)
{
const std::string param_base_name = "joint_limits." + joint_name;
Expand Down Expand Up @@ -216,7 +259,7 @@ inline bool getJointLimits(
* \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and
* \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise.
*/
inline bool getSoftJointLimits(
inline bool get_soft_joint_limits(
const std::string & joint_name, const rclcpp::Node::SharedPtr & node,
SoftJointLimits & soft_limits)
{
Expand Down Expand Up @@ -265,6 +308,6 @@ inline bool getSoftJointLimits(
return false;
}

} // namespace joint_limits_interface
} // namespace joint_limits

#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_
#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_

0 comments on commit b829d81

Please sign in to comment.