1#!/usr/bin/env python3
2# -*- coding: utf-8 -*-
3"""
4Admittance Controller Demo for ROS2 Force Control
5"""
6
7import rclpy
8from rclpy.node import Node
9from geometry_msgs.msg import Wrench, TwistStamped
10from sensor_msgs.msg import JointState
11import numpy as np
12import math
13
14class AdmittanceControlDemo(Node):
15    def __init__(self):
16        super().__init__('admittance_control_demo')
17        
18        # Publishers
19        self.admittance_pub = self.create_publisher(
20            TwistStamped, '/admittance_controller/reference', 10)
21        
22        # Subscribers
23        self.ft_sub = self.create_subscription(
24            Wrench, '/force_torque_sensor_broadcaster/wrench', 
25            self.force_callback, 10)
26        
27        # Admittance parameters (M, D, K matrices)
28        self.mass = np.diag([10.0, 10.0, 10.0, 1.0, 1.0, 1.0])
29        self.damping = np.diag([100.0, 100.0, 100.0, 10.0, 10.0, 10.0])
30        self.stiffness = np.diag([500.0, 500.0, 500.0, 50.0, 50.0, 50.0])
31        
32        # Safety limits
33        self.max_force = 50.0  # N
34        self.max_velocity = 0.1  # m/s
35
36    def force_callback(self, msg):
37        """Force/torque sensor callback"""
38        self.current_wrench = msg
39        
40        # Safety check
41        force_magnitude = math.sqrt(
42            msg.force.x**2 + msg.force.y**2 + msg.force.z**2)
43        
44        if force_magnitude > self.max_force:
45            self.emergency_stop()
46            return
47        
48        # Compute admittance response
49        self.compute_admittance_response(msg)
50
51    def compute_admittance_response(self, wrench):
52        """Calculate admittance response based on force input"""
53        # Admittance equation: M*ẍ + D*ẋ + K*x = F
54        force_vector = np.array([
55            wrench.force.x, wrench.force.y, wrench.force.z,
56            wrench.torque.x, wrench.torque.y, wrench.torque.z
57        ])
58        
59        # Calculate desired acceleration
60        desired_acc = np.linalg.inv(self.mass) @ (
61            force_vector - self.damping @ current_vel - self.stiffness @ current_pos
62        )
63        
64        # Publish velocity command
65        self.publish_velocity_command(desired_vel)