mirror of
https://github.com/huggingface/lerobot.git
synced 2026-05-18 10:10:08 +00:00
Pepijn
80 lines
2.6 KiB
Python
80 lines
2.6 KiB
Python
#!/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()
|