First, we want an using firmware with the minimum of features:

• bring the food from the plate to the mouth at the user’s mouth
• clear out the plate
• save the mouth position

Our strategy is to learn every position that the robot should reach during the meal and send to the actuator each targeted position.

For that we made an Inverse Kinematic model on the 3^rd 3 axis of our robot.

Then other, get their command position from other to keep spoon horizontally and in front of the user.

From our CAD model, we create an URDF file of our robot. With that new model, we have made simulation with PyBullet.
We were able to test our Inverse Kinematic Model.

From that simulation we learn each position that actuator should reach. That’s why using Dynamixel actuator is very interesting we don’t need to write firmware to control properly the motor everything is settle. We just need to know the position that we need to reach. After learning each position that actuator should reach, we just need a function to send target joint position.

Before using this firmware, we made a calibration firmware to set each actuator to be like our model, where output is calibration parameters that able to transform joint position in our model to actuator command and reciprocally actuator position to joint model position. It’s was little bit tricky due to our mechanical design 3^rd joint position depends on 2^nd and 3^rd actuator position.