Frame

Get the relative code from asBrain_Driver.

Joint Cmd

1. Set up the joint_config.yaml

joints:
    id_1:
        type: "RS01"      # joint type: RS00 RS01 RS02 RS03 RS04 RS05 RS06 CG
        mode: "P"         # P: position control, V: velocity control, T: torque control
        cmd: 0.0          # command value, position in rad, velocity in rad/s, torque in Nm
        kp: 10.0          # position gain
        kd: 5.0           # velocity gain
        setzero: "Open"   # Open: set zero position, Close: do not set zero position(2 encoders)
        state: 0
        pos_range:[-12.57, 12.57]
    # id_x ...

• Control Mode

	P (Position Control)	V(Velocity Control)	T(Torque Control)
Expected angle	cmd	0	0
Expected speed	0	cmd	0
Expected torque	0	0	cmd
Kp	kp	0	0
Kd	kd	kd	0

• Zero Position

1. Init the Joint

• Initialization is required every time power is turned on.

joint_joy_controller = Joint_Joy_Controller(port="/dev/ttyACM1")  # Also requires to change the Com Board
# Get the current directory
current_dir = os.path.dirname(os.path.abspath(__file__))
# Load joint configuration
joint_config_path = os.path.join(current_dir, 'joint_config_demo.yaml')
joints = load_joint_config(joint_config_path)
# Extract joint configurations
id_1 = joints.get('id_1', {})
id_2 = joints.get('id_2', {})
id_3 = joints.get('id_3', {})
id_4 = joints.get('id_4', {})
id_5 = joints.get('id_5', {})
id_6 = joints.get('id_6', {})
id_7 = joints.get('id_7', {})
id_8 = joints.get('id_8', {})
id_9 = joints.get('id_9', {})
id_10 = joints.get('id_10', {})
id_11 = joints.get('id_11', {})
id_12 = joints.get('id_12', {})
id_13 = joints.get('id_13', {})
id_14 = joints.get('id_14', {})
id_15 = joints.get('id_15', {})
id_16 = joints.get('id_16', {})
# Set joint types
joint_joy_controller.joint_type = [
    id_1["type"],  id_2["type"],  id_3["type"],  id_4["type"],
    id_9["type"],  id_10["type"], id_11["type"], id_12["type"],
    id_5["type"],  id_6["type"],  id_7["type"],  id_8["type"],
    id_13["type"], id_14["type"], id_15["type"], id_16["type"]
]    

# Initialize joints
joint_joy_controller.init_joint(
    joint_type = [id_1["type"], id_5['type'], id_9['type'],  id_13['type'],
                  id_2["type"], id_6['type'], id_10['type'], id_14['type'],
                  id_3["type"], id_7['type'], id_11['type'], id_15['type'],
                  id_4["type"], id_8['type'], id_12['type'], id_16['type']],
    joint_mode = [id_1["mode"], id_5['mode'], id_9['mode'],  id_13['mode'],
                  id_2["mode"], id_6['mode'], id_10['mode'], id_14['mode'],
                  id_3["mode"], id_7['mode'], id_11['mode'], id_15['mode'],
                  id_4["mode"], id_8['mode'], id_12['mode'], id_16['mode']],
    joint_kp = [id_1["kp"], id_5['kp'], id_9['kp'],  id_13['kp'],
                id_2["kp"], id_6['kp'], id_10['kp'], id_14['kp'],
                id_3["kp"], id_7['kp'], id_11['kp'], id_15['kp'],
                id_4["kp"], id_8['kp'], id_12['kp'], id_16['kp']],
    joint_kd = [id_1["kd"], id_5['kd'], id_9['kd'],  id_13['kd'],
                id_2["kd"], id_6['kd'], id_10['kd'], id_14['kd'],
                id_3["kd"], id_7['kd'], id_11['kd'], id_15['kd'],
                id_4["kd"], id_8['kd'], id_12['kd'], id_16['kd']],
    joint_setzero = [id_1["setzero"], id_5['setzero'], id_9['setzero'],  id_13['setzero'],
                     id_2["setzero"], id_6['setzero'], id_10['setzero'], id_14['setzero'],
                     id_3["setzero"], id_7['setzero'], id_11['setzero'], id_15['setzero'],
                     id_4["setzero"], id_8['setzero'], id_12['setzero'], id_16['setzero']]
)

1. Send the cmd

• Every powered motor needs to be sent a command (default is 0.0)

# Demo
for n in range(400 * 2):

    time_start_0 = time.time()

    # target_pos = (90 * (math.cos(1/2 * math.pi * n * 0.02) - 1)) / 180 * math.pi 
    target_pos = 0.0
    target_tor = 0.0
    # target_vel = 180  / 180 * math.pi
    # target_tor = 90 * math.sin(1/2 * math.pi * n * 0.02) / 180 * math.pi

    id_1['cmd'] = target_pos
    id_2['cmd'] = target_pos
    id_3['cmd'] = target_pos
    id_4['cmd'] = target_pos

    id_5['cmd'] = target_pos
    id_6['cmd'] = target_pos
    id_7['cmd'] = target_pos
    id_8['cmd'] = target_pos

    id_9['cmd'] = target_tor
    id_10['cmd'] = target_tor
    id_11['cmd'] = target_tor
    id_12['cmd'] = target_pos

    id_13['cmd'] = target_pos
    id_14['cmd'] = target_pos
    id_15['cmd'] = target_pos
    id_16['cmd'] = target_pos

    joint_joy_controller.send_joint_commands(
        joint_pos_vel_tor = [id_1['cmd'], id_5['cmd'], id_9['cmd'],  id_13['cmd'],
                             id_2['cmd'], id_6['cmd'], id_10['cmd'], id_14['cmd'],
                             id_3['cmd'], id_7['cmd'], id_11['cmd'], id_15['cmd'],
                             id_4['cmd'], id_8['cmd'], id_12['cmd'], id_16['cmd']])

    while(time.time() - time_start_0 < 0.02):
        pass

joint_joy_controller.close()

Joint State

1. The data is accurate following initialization.

2. The Demo of reading the joint state.

joint_joy_controller = Joint_Joy_Controller(port="/dev/ttyACM1")  # Also requires to change the Com Board

# Get the current directory
current_dir = os.path.dirname(os.path.abspath(__file__))
# Load joint configuration
joint_config_path = os.path.join(current_dir, 'joint_config_demo.yaml')
joints = load_joint_config(joint_config_path)
# Extract joint configurations
id_1 = joints.get('id_1', {})
id_2 = joints.get('id_2', {})
id_3 = joints.get('id_3', {})
id_4 = joints.get('id_4', {})
id_5 = joints.get('id_5', {})
id_6 = joints.get('id_6', {})
id_7 = joints.get('id_7', {})
id_8 = joints.get('id_8', {})
id_9 = joints.get('id_9', {})
id_10 = joints.get('id_10', {})
id_11 = joints.get('id_11', {})
id_12 = joints.get('id_12', {})
id_13 = joints.get('id_13', {})
id_14 = joints.get('id_14', {})
id_15 = joints.get('id_15', {})
id_16 = joints.get('id_16', {})
# Set joint types
joint_joy_controller.joint_type = [
    id_1["type"],  id_2["type"],  id_3["type"],  id_4["type"],
    id_9["type"],  id_10["type"], id_11["type"], id_12["type"],
    id_5["type"],  id_6["type"],  id_7["type"],  id_8["type"],
    id_13["type"], id_14["type"], id_15["type"], id_16["type"]
]    


for i in range(30000):

    joint_joy_controller.ser.reset_input_buffer()                  # Clear serial buffer before reading
    joint_joy_state = joint_joy_controller.read_joint_joy_states() # joint_joy_state: (position, velocity, effort, target_pos, channels)
    #       [id_1, id_5, id_9,  id_13,
    #        id_2, id_6, id_10, id_14,
    #        id_3, id_7, id_11, id_15,
    #        id_4, id_8, id_12, id_16]  joint state order

    if joint_joy_state is not None:
        position, velocity, effort, target_cmd, channels = joint_joy_state
        print(f"Position: {position}")
        print(f"id_1 Position: {position[id_1['state']]}")
        print(f"Velocity: {velocity}")
        print(f"id_2 Velocity: {velocity[id_2['state']]}")
        print(f"Effort: {effort}")
        print(f"id_3 Effort: {effort[id_3['state']]}")
        print(f"Target Command: {target_cmd}")
        print(f"id_4 Target Command: {target_cmd[id_4['state']]}")
        print(f"Channels: {channels}")
        print("--------------------------------------------------")
    else:
        print("Joint state read failed.")
    
joint_joy_controller.close()

Joy State

• CH3, CH4, CH2, CH1 are axes.

• CH5, CH6, CH7, CH8, CH9, CH10 are buttons.

• CH10 is an emergency stop button. Turning it clockwise to the end stops the motor power supply. Turning it counterclockwise to the end restarts the motor power supply (wait at least 5 seconds).

Robot State

1. Set up the joint_config.yaml

# Demonstration configuration file for Box to Robot state transformations (asMagicball)
Box2Robot:
    Rotate_z_Robot_Box_deg: 90.0        # Robot(body) Frame to Box(body) Frame Rotation around Z axis in degrees
    Rotate_y_Robot_Box_deg: -90.0       # Robot(body) Frame to Box(body) Frame Rotation around Y axis in degrees
    Rotate_x_Robot_Box_deg: 0.0         # Robot(body) Frame to Box(body) Frame Rotation around X axis in degrees

    Rotate_z_Robot0_Box0_deg: 90.0      # Robot0(world) Frame to Box0(world) Frame Rotation around Z axis in degrees
    Rotate_y_Robot0_Box0_deg: 0.0       # Robot0(world) Frame to Box0(world) Frame Rotation around Y axis in degrees
    Rotate_x_Robot0_Box0_deg: 0.0       # Robot0(world) Frame to Box0(world) Frame Rotation around X axis in degrees

    Rotate_z_Box_Robot_deg: -90.0       # Box(body) Frame to Robot(body) Frame Rotation around Z axis in degrees
    Rotate_y_Box_Robot_deg: 0.0         # Box(body) Frame to Robot(body) Frame Rotation around Y axis in degrees
    Rotate_x_Box_Robot_deg: -90.0       # Box(body) Frame to Robot(body) Frame Rotation around X axis in degrees

1. The Demo of reading the joint state

if __name__ == "__main__":

    mti630 = MTI630(port='/dev/ttyACM0')

    time_cost = []

    q_0 = []
    q_1 = []
    q_2 = []
    q_3 = []
    yaw = []
    pitch = []
    roll = []
    acc_x = []
    acc_y = []
    acc_z = []
    gyro_x = []
    gyro_y = []
    gyro_z = []

    for i in range(2000):

        mti630.ser.reset_input_buffer()

        imu_state = mti630.read_imu_state()
        box_state = mti630.imu_to_box_state()
        robot_state = mti630.box_to_robot_state()

        if robot_state is not None:

            q_0.append(robot_state[0][0])
            q_1.append(robot_state[0][1])
            q_2.append(robot_state[0][2])
            q_3.append(robot_state[0][3])

            # Convert quaternion to Euler angles
            euler = list(tfs.euler.quat2euler(robot_state[0]))
            roll.append(euler[0])
            pitch.append(euler[1])
            yaw.append(euler[2])

            acc_x.append(robot_state[1][0])
            acc_y.append(robot_state[1][1])
            acc_z.append(robot_state[1][2])

            gyro_x.append(robot_state[2][0])
            gyro_y.append(robot_state[2][1])
            gyro_z.append(robot_state[2][2])

            print(robot_state[0])
            print(robot_state[1])
            print(robot_state[2])
        else:
            print('Checksum is not OK')
        
    mti630.close()