qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'gitea/ros2_jazzy' into ros2_jazzy

Browse Source
This commit is contained in:
qlin960618
2024-12-28 08:58:48 +09:00
parent b2826cdce3 7ad72b3fa1
commit f7b1ef6720
7 changed files with 107 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp
View File

@@ -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;

23
sas_robot_driver_franka/__init__.py
View File

@@ -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

3
scripts/example_gripper_control.py
View File

@@ -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)

37
scripts/publish_dumy_robot_dynmaics.py
View File

@@ -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
while not rospy.is_shutdown():
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)
rate.sleep()
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()

57
scripts/subscribe_dummy_dynamic.py
View File

@@ -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")
dynam_provider = RobotDynamicsInterface("/franka1")
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():
rospy.loginfo("Waiting for robot dynamics provider to be enabled")
rospy.sleep(1)
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)
rospy.loginfo("Subscribing to dummy robot dynamics")
rate = rospy.Rate(50)
while not rospy.is_shutdown():
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)
rospy.loginfo("EE Pose: %s", ee_pose)
rospy.loginfo("Force: %s", force)
rospy.loginfo("Torque: %s", torque)
rate.sleep()
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()

2
src/hand/qros_effector_driver_franka_hand.cpp
View File

@@ -149,7 +149,9 @@ namespace qros
{
if (!_is_connected()) throw std::runtime_error(
"[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
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

5
src/sas_robot_driver_franka_hand_node.cpp
View File

@@ -53,10 +53,10 @@ void sig_int_handler(int)
template<typename T>
void get_optional_parameter(std::shared_ptr<Node> node, const std::string &param_name, T &param)
{
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);