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

Fix/Joint Command Interfaces #3

Merged
merged 3 commits into from
Feb 22, 2023
Merged

Fix/Joint Command Interfaces #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f1870c9
Remove unused controllers config
munseng-flexiv Feb 21, 2023
8f3dbd6
Hold joints before user command arrives
munseng-flexiv Feb 21, 2023
69c7371
Update joint interface to set init positions
munseng-flexiv Feb 22, 2023
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
52 changes: 1 addition & 51 deletions flexiv_bringup/config/rizon_controllers.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ position_joint_trajectory_controller:
joints: *robot_joints
constraints:
goal_time: 0.5
stopped_velocity_tolerance: 0.05
stopped_velocity_tolerance: 0.01
joint1: { goal: 0.05}
joint2: { goal: 0.05}
joint3: { goal: 0.05}
Expand All @@ -33,53 +33,3 @@ position_joint_trajectory_controller:
stop_trajectory_duration: 0.5
state_publish_rate: 100.0
action_monitor_rate: 20.0

velocity_joint_trajectory_controller:
type: velocity_controllers/JointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.5
stopped_velocity_tolerance: 0.05
joint1: { goal: 0.05}
joint2: { goal: 0.05}
joint3: { goal: 0.05}
joint4: { goal: 0.05}
joint5: { goal: 0.05}
joint6: { goal: 0.05}
joint7: { goal: 0.05}
gains:
joint1: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
joint2: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
joint3: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
joint4: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
joint5: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
joint6: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
joint7: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
stop_trajectory_duration: 0.5
state_publish_rate: 100.0
action_monitor_rate: 20.0

effort_joint_trajectory_controller:
type: effort_controllers/JointTrajectoryController
joints: *robot_joints
constraints:
goal_time: 0.5
stopped_velocity_tolerance: 0.05
joint1: { goal: 0.05}
joint2: { goal: 0.05}
joint3: { goal: 0.05}
joint4: { goal: 0.05}
joint5: { goal: 0.05}
joint6: { goal: 0.05}
joint7: { goal: 0.05}
gains:
joint1: { p: 3000, d: 80, i: 0 }
joint2: { p: 3000, d: 80, i: 0 }
joint3: { p: 800, d: 40, i: 0 }
joint4: { p: 800, d: 40, i: 0 }
joint5: { p: 200, d: 8, i: 0 }
joint6: { p: 200, d: 8, i: 0 }
joint7: { p: 200, d: 8, i: 0 }
stop_trajectory_duration: 0.5
state_publish_rate: 100.0
action_monitor_rate: 20.0
42 changes: 22 additions & 20 deletions flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW
public:
/**
* @brief Construct a new FlexivHardwareInterface object
*
*/
FlexivHardwareInterface();

Expand All @@ -48,7 +47,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW
* @brief Handles the setup functionality for the ROS interface. This
* includes parsing ROS parameters, connecting to the robot, and setting up
* hardware interfaces for ros_control.
*
* @param[in] root_nh Root level ROS node handle.
* @param[in] robot_hw_nh ROS node handle for the robot namespace.
* @return True if successful, false otherwise.
Expand All @@ -59,7 +57,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW
/**
* @brief Reads the parameterization of the hardware class from the ROS
* parameter server (e.g. local_ip, robot_ip, joint_names etc.)
*
* @param[in] root_nh A node handle in the root namespace of the control
* node.
* @param[in] robot_hw_nh A node handle in the namespace of the robot
Expand All @@ -71,7 +68,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW

/**
* @brief Initializes the class in terms of ros_control interfaces.
*
* @note You have to call initParameters beforehand. Use the complete
* initialization routine \ref init() method to control robots.
* @param[in] robot_hw_nh A node handle in the namespace of the robot
Expand All @@ -81,7 +77,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW

/**
* @brief Reads data from the robot.
*
* @param[in] time Current time.
* @param[in] period Duration of current control loop iteration.
*/
Expand All @@ -90,7 +85,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW

/**
* @brief Writes data to the robot.
*
* @param[in] time Current time.
* @param[in] period Duration of current control loop iteration.
*/
Expand All @@ -99,13 +93,11 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW

/**
* @brief Set all members to default values.
*
*/
virtual void reset();

/**
* @brief Prepares switching between controllers.
*
* @param[in] start_list List of requested controllers to start.
* @param[in] stop_list List of requested controllers to stop.
* @return True if the preparation has been successful, false otherwise.
Expand All @@ -117,7 +109,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW

/**
* @brief Perform controllers switching.
*
* @param[in] start_list List of requested controllers to start.
* @param[in] stop_list List of requested controllers to stop.
*/
Expand All @@ -126,31 +117,43 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW
const std::list<hardware_interface::ControllerInfo>& stop_list)
override;

/** @brief Enforce limits on position, velocity, and effort.
*
/**
* @brief Enforce limits on position, velocity, and effort.
* @param[in] period The duration of the current cycle.
*/
virtual void enforceLimits(const ros::Duration& period);

protected:
/**
* @brief Uses the robot_ip to connect to the robot via RDK.
*
* @return True if successful, false otherwise.
*/
virtual bool initRobot();

/**
* @brief Get current joint position and set the joint command with those
* values.
*
*/
virtual void setInitPosition();

/**
* @brief Checks whether a vector of doubles contains NaN values.
* @param[in] vector The vector to check.
* @return True if the vector contains NaN values, false otherwise.
*/
static bool vectorHasNan(const std::vector<double>& vector)
{
for (const double& value : vector) {
if (std::isnan(value)) {
return true;
}
}
return false;
}

/**
* @brief Configures a limit interface to enforce limits on effort, velocity
* or position level on joint commands.
*
* @param[in] limits_interface Limit interface for the robot.
* @param[in] command_interface Command interface to match with the limit
* interface.
Expand Down Expand Up @@ -198,7 +201,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW
/**
* @brief Checks whether a resource list contains joints from this hardware
* interface.
*
* @param[in] claimed_resources List of claimed resources.
* @return True if the list contains joints from this hardware interface,
* false otherwise.
Expand All @@ -209,7 +211,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW
/**
* @brief Publishes external force applied on TCP in TCP frame \f$
* ^{TCP}F_{ext}~[N][Nm] \f$ and base frame \f$ ^{0}F_{ext}~[N][Nm] \f$.
*
*/
virtual void publishExternalForce();

Expand Down Expand Up @@ -239,6 +240,7 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW
std::vector<double> joint_position_state_;
std::vector<double> joint_velocity_state_;
std::vector<double> joint_effort_state_;

// External force
std::vector<double> ext_force_in_tcp_;
std::vector<double> ext_force_in_base_;
Expand All @@ -250,10 +252,10 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW
std::vector<double> internal_joint_position_command_;

// Controller
bool position_controller_running_;
bool velocity_controller_running_;
bool effort_controller_running_;
bool controllers_initialized_;
std::atomic<bool> position_controller_running_;
std::atomic<bool> velocity_controller_running_;
std::atomic<bool> effort_controller_running_;
std::atomic<bool> controllers_initialized_;

// Publisher
std::unique_ptr<
Expand Down
Loading