47 lines
1.5 KiB
Python
47 lines
1.5 KiB
Python
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__":
|
|
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
|
|
node.get_logger().info("Dummy robot dynamics server started")
|
|
r = dql.DQ([0, 0, 0, 1])
|
|
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()
|
|
|