dxl_test dynamixel_client, modify eval_bc for real
这个提交包含在:
4
dxl_test.py
普通文件
4
dxl_test.py
普通文件
@ -0,0 +1,4 @@
|
|||||||
|
from dynamixel_client import DynamixelClient
|
||||||
|
client = DynamixelClient([1, 2], port='/dev/ttyDXL_wheels', lazy_connect=True)
|
||||||
|
|
||||||
|
print(client.read_pos_vel_cur())
|
||||||
604
dynamixel_client.py
普通文件
604
dynamixel_client.py
普通文件
@ -0,0 +1,604 @@
|
|||||||
|
"""Communication using the DynamixelSDK."""
|
||||||
|
##This is based off of the dynamixel SDK
|
||||||
|
import atexit
|
||||||
|
import logging
|
||||||
|
import time
|
||||||
|
from typing import Optional, Sequence, Union, Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
PROTOCOL_VERSION = 2.0
|
||||||
|
|
||||||
|
# The following addresses assume XH motors.
|
||||||
|
ADDR_TORQUE_ENABLE = 64
|
||||||
|
ADDR_GOAL_POSITION = 116
|
||||||
|
ADDR_PRESENT_POSITION = 132
|
||||||
|
ADDR_PRESENT_VELOCITY = 128
|
||||||
|
ADDR_PRESENT_CURRENT = 126
|
||||||
|
ADDR_PRESENT_POS_VEL_CUR = 126
|
||||||
|
|
||||||
|
# Data Byte Length
|
||||||
|
LEN_PRESENT_POSITION = 4
|
||||||
|
LEN_PRESENT_VELOCITY = 4
|
||||||
|
LEN_PRESENT_CURRENT = 2
|
||||||
|
LEN_PRESENT_POS_VEL_CUR = 10
|
||||||
|
LEN_GOAL_POSITION = 4
|
||||||
|
|
||||||
|
DEFAULT_POS_SCALE = 2.0 * np.pi / 4096 # 0.088 degrees
|
||||||
|
# See http://emanual.robotis.com/docs/en/dxl/x/xh430-v210/#goal-velocity
|
||||||
|
DEFAULT_VEL_SCALE = 0.229 * 2.0 * np.pi / 60.0 # 0.229 rpm
|
||||||
|
DEFAULT_CUR_SCALE = 1.34
|
||||||
|
|
||||||
|
|
||||||
|
def dynamixel_cleanup_handler():
|
||||||
|
"""Cleanup function to ensure Dynamixels are disconnected properly."""
|
||||||
|
open_clients = list(DynamixelClient.OPEN_CLIENTS)
|
||||||
|
for open_client in open_clients:
|
||||||
|
if open_client.port_handler.is_using:
|
||||||
|
logging.warning('Forcing client to close.')
|
||||||
|
open_client.port_handler.is_using = False
|
||||||
|
open_client.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
def signed_to_unsigned(value: int, size: int) -> int:
|
||||||
|
"""Converts the given value to its unsigned representation."""
|
||||||
|
if value < 0:
|
||||||
|
bit_size = 8 * size
|
||||||
|
max_value = (1 << bit_size) - 1
|
||||||
|
value = max_value + value
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
def unsigned_to_signed(value: int, size: int) -> int:
|
||||||
|
"""Converts the given value from its unsigned representation."""
|
||||||
|
bit_size = 8 * size
|
||||||
|
if (value & (1 << (bit_size - 1))) != 0:
|
||||||
|
value = -((1 << bit_size) - value)
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
class DynamixelClient:
|
||||||
|
"""Client for communicating with Dynamixel motors.
|
||||||
|
|
||||||
|
NOTE: This only supports Protocol 2.
|
||||||
|
"""
|
||||||
|
|
||||||
|
# The currently open clients.
|
||||||
|
OPEN_CLIENTS = set()
|
||||||
|
|
||||||
|
def __init__(self,
|
||||||
|
motor_ids: Sequence[int],
|
||||||
|
port: str = '/dev/ttyUSB0',
|
||||||
|
baudrate: int = 1000000,
|
||||||
|
lazy_connect: bool = False,
|
||||||
|
pos_scale: Optional[float] = None,
|
||||||
|
vel_scale: Optional[float] = None,
|
||||||
|
cur_scale: Optional[float] = None):
|
||||||
|
"""Initializes a new client.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
motor_ids: All motor IDs being used by the client.
|
||||||
|
port: The Dynamixel device to talk to. e.g.
|
||||||
|
- Linux: /dev/ttyUSB0
|
||||||
|
- Mac: /dev/tty.usbserial-*
|
||||||
|
- Windows: COM1
|
||||||
|
baudrate: The Dynamixel baudrate to communicate with.
|
||||||
|
lazy_connect: If True, automatically connects when calling a method
|
||||||
|
that requires a connection, if not already connected.
|
||||||
|
pos_scale: The scaling factor for the positions. This is
|
||||||
|
motor-dependent. If not provided, uses the default scale.
|
||||||
|
vel_scale: The scaling factor for the velocities. This is
|
||||||
|
motor-dependent. If not provided uses the default scale.
|
||||||
|
cur_scale: The scaling factor for the currents. This is
|
||||||
|
motor-dependent. If not provided uses the default scale.
|
||||||
|
"""
|
||||||
|
import dynamixel_sdk
|
||||||
|
self.dxl = dynamixel_sdk
|
||||||
|
|
||||||
|
self.motor_ids = list(motor_ids)
|
||||||
|
self.port_name = port
|
||||||
|
self.baudrate = baudrate
|
||||||
|
self.lazy_connect = lazy_connect
|
||||||
|
|
||||||
|
self.port_handler = self.dxl.PortHandler(port)
|
||||||
|
self.packet_handler = self.dxl.PacketHandler(PROTOCOL_VERSION)
|
||||||
|
|
||||||
|
self._pos_vel_cur_reader = DynamixelPosVelCurReader(
|
||||||
|
self,
|
||||||
|
self.motor_ids,
|
||||||
|
pos_scale=pos_scale if pos_scale is not None else DEFAULT_POS_SCALE,
|
||||||
|
vel_scale=vel_scale if vel_scale is not None else DEFAULT_VEL_SCALE,
|
||||||
|
cur_scale=cur_scale if cur_scale is not None else DEFAULT_CUR_SCALE,
|
||||||
|
)
|
||||||
|
self._pos_reader = DynamixelPosReader(
|
||||||
|
self,
|
||||||
|
self.motor_ids,
|
||||||
|
pos_scale=pos_scale if pos_scale is not None else DEFAULT_POS_SCALE,
|
||||||
|
vel_scale=vel_scale if vel_scale is not None else DEFAULT_VEL_SCALE,
|
||||||
|
cur_scale=cur_scale if cur_scale is not None else DEFAULT_CUR_SCALE,
|
||||||
|
)
|
||||||
|
self._vel_reader = DynamixelVelReader(
|
||||||
|
self,
|
||||||
|
self.motor_ids,
|
||||||
|
pos_scale=pos_scale if pos_scale is not None else DEFAULT_POS_SCALE,
|
||||||
|
vel_scale=vel_scale if vel_scale is not None else DEFAULT_VEL_SCALE,
|
||||||
|
cur_scale=cur_scale if cur_scale is not None else DEFAULT_CUR_SCALE,
|
||||||
|
)
|
||||||
|
self._cur_reader = DynamixelCurReader(
|
||||||
|
self,
|
||||||
|
self.motor_ids,
|
||||||
|
pos_scale=pos_scale if pos_scale is not None else DEFAULT_POS_SCALE,
|
||||||
|
vel_scale=vel_scale if vel_scale is not None else DEFAULT_VEL_SCALE,
|
||||||
|
cur_scale=cur_scale if cur_scale is not None else DEFAULT_CUR_SCALE,
|
||||||
|
)
|
||||||
|
self._sync_writers = {}
|
||||||
|
|
||||||
|
self.OPEN_CLIENTS.add(self)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_connected(self) -> bool:
|
||||||
|
return self.port_handler.is_open
|
||||||
|
|
||||||
|
def connect(self):
|
||||||
|
"""Connects to the Dynamixel motors.
|
||||||
|
|
||||||
|
NOTE: This should be called after all DynamixelClients on the same
|
||||||
|
process are created.
|
||||||
|
"""
|
||||||
|
assert not self.is_connected, 'Client is already connected.'
|
||||||
|
|
||||||
|
if self.port_handler.openPort():
|
||||||
|
logging.info('Succeeded to open port: %s', self.port_name)
|
||||||
|
else:
|
||||||
|
raise OSError(
|
||||||
|
('Failed to open port at {} (Check that the device is powered '
|
||||||
|
'on and connected to your computer).').format(self.port_name))
|
||||||
|
|
||||||
|
if self.port_handler.setBaudRate(self.baudrate):
|
||||||
|
logging.info('Succeeded to set baudrate to %d', self.baudrate)
|
||||||
|
else:
|
||||||
|
raise OSError(
|
||||||
|
('Failed to set the baudrate to {} (Ensure that the device was '
|
||||||
|
'configured for this baudrate).').format(self.baudrate))
|
||||||
|
|
||||||
|
# Start with all motors enabled. NO, I want to set settings before enabled
|
||||||
|
#self.set_torque_enabled(self.motor_ids, True)
|
||||||
|
|
||||||
|
def disconnect(self):
|
||||||
|
"""Disconnects from the Dynamixel device."""
|
||||||
|
if not self.is_connected:
|
||||||
|
return
|
||||||
|
if self.port_handler.is_using:
|
||||||
|
logging.error('Port handler in use; cannot disconnect.')
|
||||||
|
return
|
||||||
|
# Ensure motors are disabled at the end.
|
||||||
|
self.set_torque_enabled(self.motor_ids, False, retries=0)
|
||||||
|
self.port_handler.closePort()
|
||||||
|
if self in self.OPEN_CLIENTS:
|
||||||
|
self.OPEN_CLIENTS.remove(self)
|
||||||
|
|
||||||
|
def set_torque_enabled(self,
|
||||||
|
motor_ids: Sequence[int],
|
||||||
|
enabled: bool,
|
||||||
|
retries: int = -1,
|
||||||
|
retry_interval: float = 0.25):
|
||||||
|
"""Sets whether torque is enabled for the motors.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
motor_ids: The motor IDs to configure.
|
||||||
|
enabled: Whether to engage or disengage the motors.
|
||||||
|
retries: The number of times to retry. If this is <0, will retry
|
||||||
|
forever.
|
||||||
|
retry_interval: The number of seconds to wait between retries.
|
||||||
|
"""
|
||||||
|
remaining_ids = list(motor_ids)
|
||||||
|
while remaining_ids:
|
||||||
|
remaining_ids = self.write_byte(
|
||||||
|
remaining_ids,
|
||||||
|
int(enabled),
|
||||||
|
ADDR_TORQUE_ENABLE,
|
||||||
|
)
|
||||||
|
if remaining_ids:
|
||||||
|
logging.error('Could not set torque %s for IDs: %s',
|
||||||
|
'enabled' if enabled else 'disabled',
|
||||||
|
str(remaining_ids))
|
||||||
|
if retries == 0:
|
||||||
|
break
|
||||||
|
time.sleep(retry_interval)
|
||||||
|
retries -= 1
|
||||||
|
|
||||||
|
def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||||
|
"""Returns the current positions and velocities."""
|
||||||
|
return self._pos_vel_cur_reader.read()
|
||||||
|
def read_pos(self) -> np.ndarray:
|
||||||
|
"""Returns the current positions and velocities."""
|
||||||
|
return self._pos_reader.read()
|
||||||
|
def read_vel(self) -> np.ndarray:
|
||||||
|
"""Returns the current positions and velocities."""
|
||||||
|
return self._vel_reader.read()
|
||||||
|
def read_cur(self) -> np.ndarray:
|
||||||
|
"""Returns the current positions and velocities."""
|
||||||
|
return self._cur_reader.read()
|
||||||
|
|
||||||
|
def write_desired_pos(self, motor_ids: Sequence[int],
|
||||||
|
positions: np.ndarray):
|
||||||
|
"""Writes the given desired positions.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
motor_ids: The motor IDs to write to.
|
||||||
|
positions: The joint angles in radians to write.
|
||||||
|
"""
|
||||||
|
assert len(motor_ids) == len(positions)
|
||||||
|
|
||||||
|
# Convert to Dynamixel position space.
|
||||||
|
positions = positions / self._pos_vel_cur_reader.pos_scale
|
||||||
|
self.sync_write(motor_ids, positions, ADDR_GOAL_POSITION,
|
||||||
|
LEN_GOAL_POSITION)
|
||||||
|
|
||||||
|
def write_byte(
|
||||||
|
self,
|
||||||
|
motor_ids: Sequence[int],
|
||||||
|
value: int,
|
||||||
|
address: int,
|
||||||
|
) -> Sequence[int]:
|
||||||
|
"""Writes a value to the motors.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
motor_ids: The motor IDs to write to.
|
||||||
|
value: The value to write to the control table.
|
||||||
|
address: The control table address to write to.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
A list of IDs that were unsuccessful.
|
||||||
|
"""
|
||||||
|
self.check_connected()
|
||||||
|
errored_ids = []
|
||||||
|
for motor_id in motor_ids:
|
||||||
|
comm_result, dxl_error = self.packet_handler.write1ByteTxRx(
|
||||||
|
self.port_handler, motor_id, address, value)
|
||||||
|
success = self.handle_packet_result(
|
||||||
|
comm_result, dxl_error, motor_id, context='write_byte')
|
||||||
|
if not success:
|
||||||
|
errored_ids.append(motor_id)
|
||||||
|
return errored_ids
|
||||||
|
|
||||||
|
def sync_write(self, motor_ids: Sequence[int],
|
||||||
|
values: Sequence[Union[int, float]], address: int,
|
||||||
|
size: int):
|
||||||
|
"""Writes values to a group of motors.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
motor_ids: The motor IDs to write to.
|
||||||
|
values: The values to write.
|
||||||
|
address: The control table address to write to.
|
||||||
|
size: The size of the control table value being written to.
|
||||||
|
"""
|
||||||
|
self.check_connected()
|
||||||
|
key = (address, size)
|
||||||
|
if key not in self._sync_writers:
|
||||||
|
self._sync_writers[key] = self.dxl.GroupSyncWrite(
|
||||||
|
self.port_handler, self.packet_handler, address, size)
|
||||||
|
sync_writer = self._sync_writers[key]
|
||||||
|
|
||||||
|
errored_ids = []
|
||||||
|
for motor_id, desired_pos in zip(motor_ids, values):
|
||||||
|
value = signed_to_unsigned(int(desired_pos), size=size)
|
||||||
|
value = value.to_bytes(size, byteorder='little')
|
||||||
|
success = sync_writer.addParam(motor_id, value)
|
||||||
|
if not success:
|
||||||
|
errored_ids.append(motor_id)
|
||||||
|
|
||||||
|
if errored_ids:
|
||||||
|
logging.error('Sync write failed for: %s', str(errored_ids))
|
||||||
|
|
||||||
|
comm_result = sync_writer.txPacket()
|
||||||
|
self.handle_packet_result(comm_result, context='sync_write')
|
||||||
|
|
||||||
|
sync_writer.clearParam()
|
||||||
|
|
||||||
|
def check_connected(self):
|
||||||
|
"""Ensures the robot is connected."""
|
||||||
|
if self.lazy_connect and not self.is_connected:
|
||||||
|
self.connect()
|
||||||
|
if not self.is_connected:
|
||||||
|
raise OSError('Must call connect() first.')
|
||||||
|
|
||||||
|
def handle_packet_result(self,
|
||||||
|
comm_result: int,
|
||||||
|
dxl_error: Optional[int] = None,
|
||||||
|
dxl_id: Optional[int] = None,
|
||||||
|
context: Optional[str] = None):
|
||||||
|
"""Handles the result from a communication request."""
|
||||||
|
error_message = None
|
||||||
|
if comm_result != self.dxl.COMM_SUCCESS:
|
||||||
|
error_message = self.packet_handler.getTxRxResult(comm_result)
|
||||||
|
elif dxl_error is not None:
|
||||||
|
error_message = self.packet_handler.getRxPacketError(dxl_error)
|
||||||
|
if error_message:
|
||||||
|
if dxl_id is not None:
|
||||||
|
error_message = '[Motor ID: {}] {}'.format(
|
||||||
|
dxl_id, error_message)
|
||||||
|
if context is not None:
|
||||||
|
error_message = '> {}: {}'.format(context, error_message)
|
||||||
|
logging.error(error_message)
|
||||||
|
return False
|
||||||
|
return True
|
||||||
|
|
||||||
|
def convert_to_unsigned(self, value: int, size: int) -> int:
|
||||||
|
"""Converts the given value to its unsigned representation."""
|
||||||
|
if value < 0:
|
||||||
|
max_value = (1 << (8 * size)) - 1
|
||||||
|
value = max_value + value
|
||||||
|
return value
|
||||||
|
|
||||||
|
def __enter__(self):
|
||||||
|
"""Enables use as a context manager."""
|
||||||
|
if not self.is_connected:
|
||||||
|
self.connect()
|
||||||
|
return self
|
||||||
|
|
||||||
|
def __exit__(self, *args):
|
||||||
|
"""Enables use as a context manager."""
|
||||||
|
self.disconnect()
|
||||||
|
|
||||||
|
def __del__(self):
|
||||||
|
"""Automatically disconnect on destruction."""
|
||||||
|
self.disconnect()
|
||||||
|
|
||||||
|
|
||||||
|
class DynamixelReader:
|
||||||
|
"""Reads data from Dynamixel motors.
|
||||||
|
|
||||||
|
This wraps a GroupBulkRead from the DynamixelSDK.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, client: DynamixelClient, motor_ids: Sequence[int],
|
||||||
|
address: int, size: int):
|
||||||
|
"""Initializes a new reader."""
|
||||||
|
self.client = client
|
||||||
|
self.motor_ids = motor_ids
|
||||||
|
self.address = address
|
||||||
|
self.size = size
|
||||||
|
self._initialize_data()
|
||||||
|
|
||||||
|
self.operation = self.client.dxl.GroupBulkRead(client.port_handler,
|
||||||
|
client.packet_handler)
|
||||||
|
|
||||||
|
for motor_id in motor_ids:
|
||||||
|
success = self.operation.addParam(motor_id, address, size)
|
||||||
|
if not success:
|
||||||
|
raise OSError(
|
||||||
|
'[Motor ID: {}] Could not add parameter to bulk read.'
|
||||||
|
.format(motor_id))
|
||||||
|
|
||||||
|
def read(self, retries: int = 1):
|
||||||
|
"""Reads data from the motors."""
|
||||||
|
self.client.check_connected()
|
||||||
|
success = False
|
||||||
|
while not success and retries >= 0:
|
||||||
|
comm_result = self.operation.txRxPacket()
|
||||||
|
success = self.client.handle_packet_result(
|
||||||
|
comm_result, context='read')
|
||||||
|
retries -= 1
|
||||||
|
|
||||||
|
# If we failed, send a copy of the previous data.
|
||||||
|
if not success:
|
||||||
|
return self._get_data()
|
||||||
|
|
||||||
|
errored_ids = []
|
||||||
|
for i, motor_id in enumerate(self.motor_ids):
|
||||||
|
# Check if the data is available.
|
||||||
|
available = self.operation.isAvailable(motor_id, self.address,
|
||||||
|
self.size)
|
||||||
|
if not available:
|
||||||
|
errored_ids.append(motor_id)
|
||||||
|
continue
|
||||||
|
|
||||||
|
self._update_data(i, motor_id)
|
||||||
|
|
||||||
|
if errored_ids:
|
||||||
|
logging.error('Bulk read data is unavailable for: %s',
|
||||||
|
str(errored_ids))
|
||||||
|
|
||||||
|
return self._get_data()
|
||||||
|
|
||||||
|
def _initialize_data(self):
|
||||||
|
"""Initializes the cached data."""
|
||||||
|
self._data = np.zeros(len(self.motor_ids), dtype=np.float32)
|
||||||
|
|
||||||
|
def _update_data(self, index: int, motor_id: int):
|
||||||
|
"""Updates the data index for the given motor ID."""
|
||||||
|
self._data[index] = self.operation.getData(motor_id, self.address,
|
||||||
|
self.size)
|
||||||
|
|
||||||
|
def _get_data(self):
|
||||||
|
"""Returns a copy of the data."""
|
||||||
|
return self._data.copy()
|
||||||
|
|
||||||
|
|
||||||
|
class DynamixelPosVelCurReader(DynamixelReader):
|
||||||
|
"""Reads positions and velocities."""
|
||||||
|
|
||||||
|
def __init__(self,
|
||||||
|
client: DynamixelClient,
|
||||||
|
motor_ids: Sequence[int],
|
||||||
|
pos_scale: float = 1.0,
|
||||||
|
vel_scale: float = 1.0,
|
||||||
|
cur_scale: float = 1.0):
|
||||||
|
super().__init__(
|
||||||
|
client,
|
||||||
|
motor_ids,
|
||||||
|
address=ADDR_PRESENT_POS_VEL_CUR,
|
||||||
|
size=LEN_PRESENT_POS_VEL_CUR,
|
||||||
|
)
|
||||||
|
self.pos_scale = pos_scale
|
||||||
|
self.vel_scale = vel_scale
|
||||||
|
self.cur_scale = cur_scale
|
||||||
|
|
||||||
|
def _initialize_data(self):
|
||||||
|
"""Initializes the cached data."""
|
||||||
|
self._pos_data = np.zeros(len(self.motor_ids), dtype=np.float32)
|
||||||
|
self._vel_data = np.zeros(len(self.motor_ids), dtype=np.float32)
|
||||||
|
self._cur_data = np.zeros(len(self.motor_ids), dtype=np.float32)
|
||||||
|
|
||||||
|
def _update_data(self, index: int, motor_id: int):
|
||||||
|
"""Updates the data index for the given motor ID."""
|
||||||
|
cur = self.operation.getData(motor_id, ADDR_PRESENT_CURRENT,
|
||||||
|
LEN_PRESENT_CURRENT)
|
||||||
|
vel = self.operation.getData(motor_id, ADDR_PRESENT_VELOCITY,
|
||||||
|
LEN_PRESENT_VELOCITY)
|
||||||
|
pos = self.operation.getData(motor_id, ADDR_PRESENT_POSITION,
|
||||||
|
LEN_PRESENT_POSITION)
|
||||||
|
cur = unsigned_to_signed(cur, size=2)
|
||||||
|
vel = unsigned_to_signed(vel, size=4)
|
||||||
|
pos = unsigned_to_signed(pos, size=4)
|
||||||
|
self._pos_data[index] = float(pos) * self.pos_scale
|
||||||
|
self._vel_data[index] = float(vel) * self.vel_scale
|
||||||
|
self._cur_data[index] = float(cur) * self.cur_scale
|
||||||
|
|
||||||
|
def _get_data(self):
|
||||||
|
"""Returns a copy of the data."""
|
||||||
|
return (self._pos_data.copy(), self._vel_data.copy(),
|
||||||
|
self._cur_data.copy())
|
||||||
|
|
||||||
|
|
||||||
|
class DynamixelPosReader(DynamixelReader):
|
||||||
|
"""Reads positions and velocities."""
|
||||||
|
|
||||||
|
def __init__(self,
|
||||||
|
client: DynamixelClient,
|
||||||
|
motor_ids: Sequence[int],
|
||||||
|
pos_scale: float = 1.0,
|
||||||
|
vel_scale: float = 1.0,
|
||||||
|
cur_scale: float = 1.0):
|
||||||
|
super().__init__(
|
||||||
|
client,
|
||||||
|
motor_ids,
|
||||||
|
address=ADDR_PRESENT_POS_VEL_CUR,
|
||||||
|
size=LEN_PRESENT_POS_VEL_CUR,
|
||||||
|
)
|
||||||
|
self.pos_scale = pos_scale
|
||||||
|
|
||||||
|
def _initialize_data(self):
|
||||||
|
"""Initializes the cached data."""
|
||||||
|
self._pos_data = np.zeros(len(self.motor_ids), dtype=np.float32)
|
||||||
|
|
||||||
|
def _update_data(self, index: int, motor_id: int):
|
||||||
|
"""Updates the data index for the given motor ID."""
|
||||||
|
pos = self.operation.getData(motor_id, ADDR_PRESENT_POSITION,
|
||||||
|
LEN_PRESENT_POSITION)
|
||||||
|
pos = unsigned_to_signed(pos, size=4)
|
||||||
|
self._pos_data[index] = float(pos) * self.pos_scale
|
||||||
|
|
||||||
|
def _get_data(self):
|
||||||
|
"""Returns a copy of the data."""
|
||||||
|
return self._pos_data.copy()
|
||||||
|
|
||||||
|
class DynamixelVelReader(DynamixelReader):
|
||||||
|
"""Reads positions and velocities."""
|
||||||
|
|
||||||
|
def __init__(self,
|
||||||
|
client: DynamixelClient,
|
||||||
|
motor_ids: Sequence[int],
|
||||||
|
pos_scale: float = 1.0,
|
||||||
|
vel_scale: float = 1.0,
|
||||||
|
cur_scale: float = 1.0):
|
||||||
|
super().__init__(
|
||||||
|
client,
|
||||||
|
motor_ids,
|
||||||
|
address=ADDR_PRESENT_POS_VEL_CUR,
|
||||||
|
size=LEN_PRESENT_POS_VEL_CUR,
|
||||||
|
)
|
||||||
|
self.pos_scale = pos_scale
|
||||||
|
self.vel_scale = vel_scale
|
||||||
|
self.cur_scale = cur_scale
|
||||||
|
|
||||||
|
def _initialize_data(self):
|
||||||
|
"""Initializes the cached data."""
|
||||||
|
self._vel_data = np.zeros(len(self.motor_ids), dtype=np.float32)
|
||||||
|
|
||||||
|
def _update_data(self, index: int, motor_id: int):
|
||||||
|
"""Updates the data index for the given motor ID."""
|
||||||
|
vel = self.operation.getData(motor_id, ADDR_PRESENT_VELOCITY,
|
||||||
|
LEN_PRESENT_VELOCITY)
|
||||||
|
vel = unsigned_to_signed(vel, size=4)
|
||||||
|
self._vel_data[index] = float(vel) * self.vel_scale
|
||||||
|
|
||||||
|
def _get_data(self):
|
||||||
|
"""Returns a copy of the data."""
|
||||||
|
return self._vel_data.copy()
|
||||||
|
|
||||||
|
class DynamixelCurReader(DynamixelReader):
|
||||||
|
"""Reads positions and velocities."""
|
||||||
|
|
||||||
|
def __init__(self,
|
||||||
|
client: DynamixelClient,
|
||||||
|
motor_ids: Sequence[int],
|
||||||
|
pos_scale: float = 1.0,
|
||||||
|
vel_scale: float = 1.0,
|
||||||
|
cur_scale: float = 1.0):
|
||||||
|
super().__init__(
|
||||||
|
client,
|
||||||
|
motor_ids,
|
||||||
|
address=ADDR_PRESENT_POS_VEL_CUR,
|
||||||
|
size=LEN_PRESENT_POS_VEL_CUR,
|
||||||
|
)
|
||||||
|
self.cur_scale = cur_scale
|
||||||
|
|
||||||
|
def _initialize_data(self):
|
||||||
|
"""Initializes the cached data."""
|
||||||
|
self._cur_data = np.zeros(len(self.motor_ids), dtype=np.float32)
|
||||||
|
|
||||||
|
def _update_data(self, index: int, motor_id: int):
|
||||||
|
"""Updates the data index for the given motor ID."""
|
||||||
|
cur = self.operation.getData(motor_id, ADDR_PRESENT_CURRENT,
|
||||||
|
LEN_PRESENT_CURRENT)
|
||||||
|
cur = unsigned_to_signed(cur, size=2)
|
||||||
|
self._cur_data[index] = float(cur) * self.cur_scale
|
||||||
|
|
||||||
|
def _get_data(self):
|
||||||
|
"""Returns a copy of the data."""
|
||||||
|
return self._cur_data.copy()
|
||||||
|
|
||||||
|
|
||||||
|
# Register global cleanup function.
|
||||||
|
atexit.register(dynamixel_cleanup_handler)
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
import argparse
|
||||||
|
import itertools
|
||||||
|
|
||||||
|
parser = argparse.ArgumentParser()
|
||||||
|
parser.add_argument(
|
||||||
|
'-m',
|
||||||
|
'--motors',
|
||||||
|
required=True,
|
||||||
|
help='Comma-separated list of motor IDs.')
|
||||||
|
parser.add_argument(
|
||||||
|
'-d',
|
||||||
|
'--device',
|
||||||
|
default='/dev/ttyUSB0',
|
||||||
|
help='The Dynamixel device to connect to.')
|
||||||
|
parser.add_argument(
|
||||||
|
'-b', '--baud', default=1000000, help='The baudrate to connect with.')
|
||||||
|
parsed_args = parser.parse_args()
|
||||||
|
|
||||||
|
motors = [int(motor) for motor in parsed_args.motors.split(',')]
|
||||||
|
|
||||||
|
way_points = [np.zeros(len(motors)), np.full(len(motors), np.pi)]
|
||||||
|
|
||||||
|
with DynamixelClient(motors, parsed_args.device,
|
||||||
|
parsed_args.baud) as dxl_client:
|
||||||
|
for step in itertools.count():
|
||||||
|
if step > 0 and step % 50 == 0:
|
||||||
|
way_point = way_points[(step // 100) % len(way_points)]
|
||||||
|
print('Writing: {}'.format(way_point.tolist()))
|
||||||
|
dxl_client.write_desired_pos(motors, way_point)
|
||||||
|
read_start = time.time()
|
||||||
|
pos_now, vel_now, cur_now = dxl_client.read_pos_vel_cur()
|
||||||
|
if step % 5 == 0:
|
||||||
|
print('[{}] Frequency: {:.2f} Hz'.format(
|
||||||
|
step, 1.0 / (time.time() - read_start)))
|
||||||
|
print('> Pos: {}'.format(pos_now.tolist()))
|
||||||
|
print('> Vel: {}'.format(vel_now.tolist()))
|
||||||
|
print('> Cur: {}'.format(cur_now.tolist()))
|
||||||
@ -120,7 +120,7 @@ def main(args):
|
|||||||
ckpt_names = [f'policy_last.ckpt']
|
ckpt_names = [f'policy_last.ckpt']
|
||||||
results = []
|
results = []
|
||||||
for ckpt_name in ckpt_names:
|
for ckpt_name in ckpt_names:
|
||||||
success_rate, avg_return = eval_bc(config, ckpt_name, save_episode=True, num_rollouts=1)
|
success_rate, avg_return = eval_bc(config, ckpt_name, save_episode=True, num_rollouts=10)
|
||||||
# wandb.log({'success_rate': success_rate, 'avg_return': avg_return})
|
# wandb.log({'success_rate': success_rate, 'avg_return': avg_return})
|
||||||
results.append([ckpt_name, success_rate, avg_return])
|
results.append([ckpt_name, success_rate, avg_return])
|
||||||
|
|
||||||
@ -237,6 +237,8 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
episode_returns = []
|
episode_returns = []
|
||||||
highest_rewards = []
|
highest_rewards = []
|
||||||
for rollout_id in range(num_rollouts):
|
for rollout_id in range(num_rollouts):
|
||||||
|
if real_robot:
|
||||||
|
e()
|
||||||
rollout_id += 0
|
rollout_id += 0
|
||||||
### set task
|
### set task
|
||||||
if 'sim_transfer_cube' in task_name:
|
if 'sim_transfer_cube' in task_name:
|
||||||
@ -316,7 +318,7 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
action = post_process(raw_action)
|
action = post_process(raw_action)
|
||||||
target_qpos = action[:-2]
|
target_qpos = action[:-2]
|
||||||
base_action = action[-2:]
|
base_action = action[-2:]
|
||||||
base_action = calibrate_linear_vel(base_action, c=0.19)
|
# base_action = calibrate_linear_vel(base_action, c=0.19)
|
||||||
# base_action = postprocess_base_action(base_action)
|
# base_action = postprocess_base_action(base_action)
|
||||||
|
|
||||||
### step the environment
|
### step the environment
|
||||||
@ -342,8 +344,8 @@ def eval_bc(config, ckpt_name, save_episode=True, num_rollouts=50):
|
|||||||
highest_rewards.append(episode_highest_reward)
|
highest_rewards.append(episode_highest_reward)
|
||||||
print(f'Rollout {rollout_id}\n{episode_return=}, {episode_highest_reward=}, {env_max_reward=}, Success: {episode_highest_reward==env_max_reward}')
|
print(f'Rollout {rollout_id}\n{episode_return=}, {episode_highest_reward=}, {env_max_reward=}, Success: {episode_highest_reward==env_max_reward}')
|
||||||
|
|
||||||
if save_episode:
|
# if save_episode:
|
||||||
save_videos(image_list, DT, video_path=os.path.join(ckpt_dir, f'video{rollout_id}.mp4'))
|
# save_videos(image_list, DT, video_path=os.path.join(ckpt_dir, f'video{rollout_id}.mp4'))
|
||||||
|
|
||||||
success_rate = np.mean(np.array(highest_rewards) == env_max_reward)
|
success_rate = np.mean(np.array(highest_rewards) == env_max_reward)
|
||||||
avg_return = np.mean(episode_returns)
|
avg_return = np.mean(episode_returns)
|
||||||
|
|||||||
8
utils.py
8
utils.py
@ -208,9 +208,13 @@ def preprocess_base_action(base_action):
|
|||||||
|
|
||||||
return base_action
|
return base_action
|
||||||
|
|
||||||
def postprocess_base_action(self, base_action):
|
def postprocess_base_action(base_action):
|
||||||
linear_vel, angular_vel = base_action
|
linear_vel, angular_vel = base_action
|
||||||
angular_vel *= 0.9
|
linear_vel *= 0.8
|
||||||
|
angular_vel *= 0.70
|
||||||
|
# angular_vel = 0
|
||||||
|
if np.abs(linear_vel) < 0.05:
|
||||||
|
linear_vel = 0
|
||||||
return np.array([linear_vel, angular_vel])
|
return np.array([linear_vel, angular_vel])
|
||||||
|
|
||||||
### env utils
|
### env utils
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户