add damiao

tests/motors/test_damiao.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# TODO(pepijn): add license of: https://github.com/cmjang/DM_Control_Python?tab=MIT-1-ov-file#readme
+
+import time
+import math
+import numpy as np
+from damiao_motor import Motor, MotorControl, DM_Motor_Type, Control_Type
+
+# Configuration parameters
+NUM_MOTORS = 1  # Number of motors to control
+CAN_INTERFACE = "can0"  # CAN interface name
+CAN_BITRATE = 1000000  # CAN bus baud rate
+MOTOR_TYPE = DM_Motor_Type.DM4310  # Motor model
+
+# Sine wave parameters
+FREQUENCY = 0.1  # Frequency (Hz)
+AMPLITUDE = 6  # Amplitude (rad)
+DURATION = 60.0  # Operation duration (s)
+
+def main():
+    # Create motor controller object
+    control = MotorControl(CAN_INTERFACE, bitrate=CAN_BITRATE)
+    
+    # Create and add motors
+    motors = []
+    for i in range(NUM_MOTORS):
+        motor = Motor(MOTOR_TYPE, i + 1, i + 0X10)  # CAN IDs start from 1
+        control.addMotor(motor)
+        motors.append(motor)
+        control.enable(motor)
+        print(f"Motor {i + 1} enabled")
+    
+    try:
+        start_time = time.time()
+        while time.time() - start_time < DURATION:
+            current_time = time.time() - start_time
+            
+            # Calculate sine wave position
+            position = AMPLITUDE * math.sin(2 * math.pi * FREQUENCY * current_time)
+            
+            # Control all motors
+            for motor in motors:
+                control.controlMIT(
+                    motor,
+                    kp=10.0,  # Position gain
+                    kd=1.0,   # Velocity gain
+                    q=position,  # Target position
+                    dq=0.0,   # Target velocity
+                    tau=0.0   # Feedforward torque
+                )
+            
+            # Control frequency
+            time.sleep(0.001)  # 1kHz control frequency
+            
+    except KeyboardInterrupt:
+        print("\nProgram interrupted by user")
+    finally:
+        # Disable all motors
+        for motor in motors:
+            control.disable(motor)
+            print(f"Motor {motor.SlaveID} disabled")
+
+if __name__ == "__main__":
+    main()