pinky.yaml

Location: FPGA, /home/root/pinky.yaml

Github: https://github.com/Roboy/roboy_plexus/blob/roboy3/config/pinky.yaml

Function

• list all busses connected to the Core Board (FPGA breakout board)

• map bus_id to global_id (system-wide unique) for all motors

• define conversion factors from meter to encoder ticks

• set spinning direction of M2

• define joint position offset for 1DOF magnetic sensors (2x per elbow, 2x per knee)

Updating motor ids

Once the motor board is exchange, one needs to update the pinky.yaml with the new id written on the motorboard (133 on the photo).

1. Find out global_id of the non-functional motor either through Node-Red’s System Check UI or using the /roboy/pinky/middleware/SystemCheck ROS service: rosservice call /roboy/pinky/middleware/SystemCheck "motorIDs: [suspected_id1, suspected_id2]"

2. Note the bus_id written by hand on the broken motorboard

3. Note the bus_id written by hand on the new functional motorboard

4. SSH to the FPGA ssh root@192.168.1.196

5. Open pinky.yaml vim pinky.yaml

6. Go to icebus section

7. Find out the (X,Y) position in the motor’s 2D list where you need to update:

   1. using the broken global_id from step 1

      1. in motor_ids_global 2D list find X and Y coordinate of the global_id

   2. using the broken bus_id from step 2

      1. using comments in motor_ids_global find out X coordinate by the body part name

      2. find out Y coordinate by the broken bus_id in the bud_ids array

8. Replace icebus -> bus_ids[X][Y] with the new motorboard bus_id from step 3

Open

Open

cardsflow.xml

model.urdf

motor_config.yaml

controller.yaml

Link: https://github.com/Roboy/roboy3_models/blob/caspr/upper_body/config/controller.yaml

Example:

• Kp: P gain for PD controller

• Kd: D gain for PD controller

• dt: time step of doing integration inside forward_kinematic function

Note: These parameters can be changed at runtime if debug:=true when starting kindyn

Control workflow

• Input: current_joints, target_joints

• Output: direct tendon length for each motor

• Workflow

  • Compute the error on joint space

    joint_error = target_joints - current_joints

  • Apply PD controller on joint_error, with Kp and Kd

    joint_command = Kp * joint_error + Kd * (joint_error - last_joint_error) / delta_t

  • Transform it to the command on tendon space

    l_command = L * joint_command // L is the Jacobian matrix

  • Doing one step forward kinematic to get the final command with parameter dt

    Note: Need to do inversion since we need both q_next and l_next; without inversion, we can not convert directly l_next to q_next since it is an ill-posed mapping.

  • Send l_next to roboy_plexus