diff --git a/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp b/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp index e26bcc0..1dcd187 100644 --- a/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp +++ b/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp @@ -69,6 +69,7 @@ namespace qros { struct EffectorDriverFrankaHandConfiguration { std::string robot_ip; + bool initialize_with_homing = true; double thread_sampeling_time_s = 1e8; // 10Hz double default_force = 3.0; double default_speed = 0.1; diff --git a/sas_robot_driver_franka/__init__.py b/sas_robot_driver_franka/__init__.py index 91e5636..50237d5 100644 --- a/sas_robot_driver_franka/__init__.py +++ b/sas_robot_driver_franka/__init__.py @@ -9,6 +9,7 @@ from rclpy.node import Node from rclpy.client import Client from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown from sas_robot_driver_franka_interfaces.srv import Move, Grasp +from std_srvs.srv import Trigger from sas_robot_driver_franka_interfaces.msg import GripperState import os import threading @@ -27,12 +28,15 @@ class FrankaGripperInterface: """ def __init__(self, node: Node, robot_prefix): + self.last_message = None self.robot_prefix = robot_prefix self.node = node self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX)) self._moving = False self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX)) self._grasping = False + self.home_service = node.create_client(Trigger, os.path.join(robot_prefix, "homing")) + self._homing = False self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX), self._status_callback, 10) @@ -71,6 +75,14 @@ class FrankaGripperInterface: self.state_max_width = msg.max_width self.state_temperature = msg.temperature self.state_is_grasped = msg.is_grasped + def home(self): + """ + Reinitialize the gripper + :return: None + """ + if self.is_busy(): + raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished") + self._home() def move(self, width, speed=0): """ @@ -158,14 +170,19 @@ class FrankaGripperInterface: self._moving = False elif isinstance(response, Grasp.Response): self._grasping = False + elif isinstance(response, Trigger.Response): + self._homing = False else: raise Exception("Invalid response type") self.service_future = None + self.last_message = hasattr(response, "message") and response.message or "" return response.success else: raise Exception("No result available") else: raise Exception("No result available") + def get_last_message(self): + return self.last_message def wait_until_done(self): """ @@ -179,6 +196,12 @@ class FrankaGripperInterface: while not self.is_done(): self.spin_handler() time.sleep(0.01) + def _home(self): + self._homing = True + # self.node.get_logger().info("Homing gripper") + request = Trigger.Request() + # self.node.get_logger().info("Calling home service") + self.service_future = self.home_service.call_async(request) def _move(self, width, speed): self._moving = True diff --git a/scripts/example_gripper_control.py b/scripts/example_gripper_control.py index f0f917a..c1a5008 100644 --- a/scripts/example_gripper_control.py +++ b/scripts/example_gripper_control.py @@ -1,6 +1,5 @@ import threading -from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown import rclpy from sas_robot_driver_franka import FrankaGripperInterface import time @@ -14,13 +13,11 @@ def main_loop(node): while not hand1.is_enabled(): logger.info("Waiting for gripper to be enabled...") - rclcpp_spin_some(node) time.sleep(0.1) rclpy.node.get_logger(node_name).info("Gripper enabled!") def spin_all(node_): while rclpy.ok(): - rclcpp_spin_some(node_) rclpy.spin_once(node_, timeout_sec=0.1) time.sleep(0.1) diff --git a/scripts/publish_dumy_robot_dynmaics.py b/scripts/publish_dumy_robot_dynmaics.py index c1e1102..0c27cf7 100644 --- a/scripts/publish_dumy_robot_dynmaics.py +++ b/scripts/publish_dumy_robot_dynmaics.py @@ -1,29 +1,46 @@ -import rospy -from sas_robot_driver_franka import RobotDynamicsServer +import time + +import rclpy +from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown +from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer import dqrobotics as dql import numpy as np -if __name__ == "__main__": - rospy.init_node("dummy_robot_dynamics_provider") - dynam_provider = RobotDynamicsProvider("/franka1") + +if __name__ == "__main__": + rclcpp_init() + rclpy.init() + sas_node = rclcpp_Node("dummy_robot_dynamics_server_sas") + node = rclpy.create_node("dummy_robot_dynamics_server") + dynam_provider = RobotDynamicsServer(sas_node, "/franka1") + t = dql.DQ([0, 0, 1]) r = dql.DQ([1, 0, 0, 0]) base_pose = r + 0.5 * dql.E_ * t * r dynam_provider.set_world_to_base_tf(base_pose) t_ = 0 - rospy.loginfo("Publishing dummy robot dynamics") + node.get_logger().info("Dummy robot dynamics server started") r = dql.DQ([0, 0, 0, 1]) - rate = rospy.Rate(100) + rate = node.create_rate(100) dummy_force = np.random.rand(3) * 100 dummy_torque = np.random.rand(3) * 10 + try: + while rclpy.ok(): + t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi) + ee_pose = r + 0.5 * dql.E_ * t * r + dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10 + dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1 + dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque) + rclcpp_spin_some(sas_node) + rclpy.spin_once(node) + t_ += 0.01 + time.sleep(0.1) + except KeyboardInterrupt: + pass + finally: + rclcpp_shutdown() + node.destroy_node() + rclpy.shutdown() - while not rospy.is_shutdown(): - t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi) - ee_pose = r + 0.5 * dql.E_ * t * r - dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10 - dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1 - dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque) - rate.sleep() - t_ += 0.01 diff --git a/scripts/subscribe_dummy_dynamic.py b/scripts/subscribe_dummy_dynamic.py index 136bba4..352bb34 100644 --- a/scripts/subscribe_dummy_dynamic.py +++ b/scripts/subscribe_dummy_dynamic.py @@ -1,30 +1,53 @@ -import rospy -from sas_robot_driver_franka import RobotDynamicsInterface +import time + +import rclpy +from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown +from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer import dqrobotics as dql import numpy as np from dqrobotics.interfaces.vrep import DQ_VrepInterface -vrep = DQ_VrepInterface() -vrep.connect("192.168.10.103", 19997, 100, 10) +# vrep = DQ_VrepInterface() +# vrep.connect("192.168.10.103", 19997, 100, 10) +vrep = None if __name__ == "__main__": - rospy.init_node("dummy_robot_dynamics_subscriber") + rclcpp_init() + rclpy.init() + sas_node = rclcpp_Node("dummy_robot_dynamics_client_sas") + node = rclpy.create_node("dummy_robot_dynamics_client") + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin_once(timeout_sec=0.1) + dynam_provider = RobotDynamicsClient(sas_node, "/franka1") + rclcpp_spin_some(sas_node) + while not dynam_provider.is_enabled(): + node.get_logger().info("Waiting for robot dynamics provider to be enabled") + rclcpp_spin_some(sas_node) + executor.spin_once(timeout_sec=0.1) + node.get_logger().info("retrying...") + time.sleep(0.5) - dynam_provider = RobotDynamicsInterface("/franka1") - while not dynam_provider.is_enabled(): - rospy.loginfo("Waiting for robot dynamics provider to be enabled") - rospy.sleep(1) - - rospy.loginfo("Subscribing to dummy robot dynamics") - rate = rospy.Rate(50) - - while not rospy.is_shutdown(): - force = dynam_provider.get_stiffness_force() - torque = dynam_provider.get_stiffness_torque() - ee_pose = dynam_provider.get_stiffness_frame_pose() - vrep.set_object_pose("xd1", ee_pose) - rospy.loginfo("EE Pose: %s", ee_pose) - rospy.loginfo("Force: %s", force) - rospy.loginfo("Torque: %s", torque) - rate.sleep() \ No newline at end of file + node.get_logger().info("Dummy robot dynamics client started") + while rclpy.ok(): + force = dynam_provider.get_stiffness_force() + torque = dynam_provider.get_stiffness_torque() + ee_pose = dynam_provider.get_stiffness_frame_pose() + if vrep is not None: + vrep.set_object_pose("xd1", ee_pose) + node.get_logger().info(f"EE Pose: {ee_pose}") + node.get_logger().info(f"Force: {force}") + node.get_logger().info(f"Torque: {torque}") + rclcpp_spin_some(sas_node) + executor.spin_once(timeout_sec=0.1) + time.sleep(0.5) + except KeyboardInterrupt: + pass + finally: + rclcpp_shutdown() + node.destroy_node() + if vrep is not None: + vrep.disconnect() + rclpy.shutdown() diff --git a/src/hand/qros_effector_driver_franka_hand.cpp b/src/hand/qros_effector_driver_franka_hand.cpp index 0c514bf..88af19a 100644 --- a/src/hand/qros_effector_driver_franka_hand.cpp +++ b/src/hand/qros_effector_driver_franka_hand.cpp @@ -149,7 +149,9 @@ namespace qros { if (!_is_connected()) throw std::runtime_error( "[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected."); - gripper_homing(); + if (configuration_.initialize_with_homing) { + gripper_homing(); + } // start gripper status loop status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this); // check status loop with timeout diff --git a/src/sas_robot_driver_franka_hand_node.cpp b/src/sas_robot_driver_franka_hand_node.cpp index 12ede61..27cca67 100644 --- a/src/sas_robot_driver_franka_hand_node.cpp +++ b/src/sas_robot_driver_franka_hand_node.cpp @@ -53,10 +53,10 @@ void sig_int_handler(int) template void get_optional_parameter(std::shared_ptr node, const std::string ¶m_name, T ¶m) { - if(node->has_parameter(param_name)) + try { sas::get_ros_parameter(node,param_name,param); - }else + }catch (const std::exception& e) { RCLCPP_INFO_STREAM(node->get_logger(), "["+std::string(node->get_name())+"]:Parameter " + param_name + " not found. Using default value. " + std::to_string(param)); } @@ -81,6 +81,7 @@ int main(int argc, char **argv) RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server."); qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration; + get_optional_parameter(node, "initialize_with_homing", robot_driver_franka_hand_configuration.initialize_with_homing); sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_hand_configuration.robot_ip); sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_franka_hand_configuration.thread_sampeling_time_s); sas::get_ros_parameter(node,"default_force",robot_driver_franka_hand_configuration.default_force);