From 1348b4779763cdc603aacad11b3cc15582401b60 Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Wed, 26 Feb 2025 11:14:32 -0500 Subject: [PATCH] ready to test on robot --- .../spot_forward_controller.hpp | 4 +- .../src/spot_forward_controller.cpp | 10 ++- .../spot_ros2_control/set_gripper_gains.py | 84 +++++++++++++++++++ 3 files changed, 95 insertions(+), 3 deletions(-) create mode 100644 spot_ros2_control/spot_ros2_control/set_gripper_gains.py diff --git a/spot_controllers/include/spot_controllers/spot_forward_controller.hpp b/spot_controllers/include/spot_controllers/spot_forward_controller.hpp index b7e7031e8..a29d3b0a6 100644 --- a/spot_controllers/include/spot_controllers/spot_forward_controller.hpp +++ b/spot_controllers/include/spot_controllers/spot_forward_controller.hpp @@ -29,8 +29,8 @@ #include "std_msgs/msg/float64_multi_array.hpp" namespace spot_controllers { -// using CmdType = spot_msgs::msg::JointCommand; -using CmdType = sensor_msgs::msg::JointState; +using CmdType = spot_msgs::msg::JointCommand; +// using CmdType = sensor_msgs::msg::JointState; /** * \brief Forward command controller for a set of joints and interfaces. diff --git a/spot_controllers/src/spot_forward_controller.cpp b/spot_controllers/src/spot_forward_controller.cpp index 3235bf761..02e7bca36 100644 --- a/spot_controllers/src/spot_forward_controller.cpp +++ b/spot_controllers/src/spot_forward_controller.cpp @@ -56,6 +56,8 @@ controller_interface::InterfaceConfiguration SpotForwardController::command_inte command_interfaces_config.names.push_back(joint + "/" + "position"); command_interfaces_config.names.push_back(joint + "/" + "velocity"); command_interfaces_config.names.push_back(joint + "/" + "effort"); + command_interfaces_config.names.push_back(joint + "/" + "k_q_p"); + command_interfaces_config.names.push_back(joint + "/" + "k_qd_p"); } return command_interfaces_config; @@ -109,6 +111,8 @@ controller_interface::return_type SpotForwardController::update(const rclcpp::Ti const bool using_position = (*joint_commands)->position.size() == njoints_to_command; const bool using_velocity = (*joint_commands)->velocity.size() == njoints_to_command; const bool using_effort = (*joint_commands)->effort.size() == njoints_to_command; + const bool using_k_q_p = (*joint_commands)->k_q_p.size() == njoints_to_command; + const bool using_k_qd_p = (*joint_commands)->k_qd_p.size() == njoints_to_command; for (size_t i = 0; i < command_interfaces_.size(); i++) { // idea: for every element in the command interface: @@ -126,13 +130,17 @@ controller_interface::return_type SpotForwardController::update(const rclcpp::Ti // get the index that the name is at in the command const auto command_index = std::distance(joint_names.begin(), it); // check if interface name is there and update accordingly - // todo(katie) (micro optimization) -- could break early none of using_position/velocity/effort is true. + // todo(katie) (micro optimization) -- could break early none of using_x fields is true. if (interface_name == "position" && using_position) { command_interfaces_.at(i).set_value((*joint_commands)->position.at(command_index)); } else if (interface_name == "velocity" && using_velocity) { command_interfaces_.at(i).set_value((*joint_commands)->velocity.at(command_index)); } else if (interface_name == "effort" && using_effort) { command_interfaces_.at(i).set_value((*joint_commands)->effort.at(command_index)); + } else if (interface_name == "k_q_p" && using_k_q_p) { + command_interfaces_.at(i).set_value((*joint_commands)->k_q_p.at(command_index)); + } else if (interface_name == "k_qd_p" && using_k_qd_p) { + command_interfaces_.at(i).set_value((*joint_commands)->k_qd_p.at(command_index)); } } diff --git a/spot_ros2_control/spot_ros2_control/set_gripper_gains.py b/spot_ros2_control/spot_ros2_control/set_gripper_gains.py new file mode 100644 index 000000000..b24ef8225 --- /dev/null +++ b/spot_ros2_control/spot_ros2_control/set_gripper_gains.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python +# Copyright (c) 2025 Boston Dynamics AI Institute LLC. All rights reserved. +import time + +import synchros2.process as ros_process +from synchros2.node import Node + +from spot_msgs.msg import JointCommand + +# maximum and minimum joint angles in radians. +GRIPPER_OPEN_ANGLE = -1.57 +GRIPPER_CLOSE_ANGLE = 0.0 + + +class ExampleGripperStreaming: + def __init__(self, node: Node) -> None: + """Initialize the example.""" + self._node = node + self._logger = self._node.get_logger() + self._command_pub = self._node.create_publisher(JointCommand, "spot_forward_controller/joint_commands", 10) + self._joint_command = JointCommand() + self._joint_command.name = ["arm_f1x"] + self._joint_command.k_q_p = [16.0] + self._joint_command.k_qd_p = [0.32] + + def get_k_q_p(self): + """Get the current k_q_p command for the gripper.""" + return self._joint_command.k_q_p[0] + + def get_k_qd_p(self): + """Get the current k_qd_p command for the gripper.""" + return self._joint_command.k_qd_p[0] + + def set_gains(self, k_q_p: float, k_qd_p: float) -> None: + """Set the k_q_p and k_qd_p commands for the gripper.""" + self._joint_command.k_q_p = [k_q_p] + self._joint_command.k_qd_p = [k_qd_p] + + def move_gripper(self, goal_joint_angle: float) -> None: + """Command the gripper to a given joint angle by streaming a command.""" + self._joint_command.position = [goal_joint_angle] + self._command_pub.publish(self._joint_command) + + def open_and_close(self, duration_sec: float = 1.0, npoints: int = 50) -> None: + """Open and close the gripper by streaming position commands. + + Args: + duration_sec (float): Duration of each open and close movement + npoints (int): number of points to send for each open and close movement. + """ + dt = duration_sec / npoints + step_size_rad = (GRIPPER_OPEN_ANGLE - GRIPPER_CLOSE_ANGLE) / npoints + self._logger.info("Opening...") + for i in range(npoints): + self.move_gripper(goal_joint_angle=(GRIPPER_CLOSE_ANGLE + i * step_size_rad)) + time.sleep(dt) + self._logger.info("Closing...") + for i in range(npoints): + self.move_gripper(goal_joint_angle=(GRIPPER_OPEN_ANGLE - i * step_size_rad)) + time.sleep(dt) + + +@ros_process.main() +def main() -> None: + """Run the example.""" + example = ExampleGripperStreaming(node=main.node) + example.open_and_close() + while True: + # Get gains to try out from the user. + k_q_p_str = input(f"Select k_q_p gain (current value: {example.get_k_q_p()}): ") + k_qd_p_str = input(f"Select k_qd_p gain (current value: {example.get_k_qd_p()}): ") + print(f"you selected k_q_p={k_q_p_str}, k_qd_p={k_qd_p_str}") + try: + k_q_p = float(k_q_p_str) + k_qd_p = float(k_qd_p_str) + example.set_gains(k_q_p, k_qd_p) + except ValueError: + print("Your inputs could not be converted to floats -- keeping the same gains.") + # open and close the gripper with the new gains set. + example.open_and_close() + + +if __name__ == "__main__": + main()