Motor angle estimation through Inverse kinematics

Hello everyone!

We have been trying out the different ways of making our reachy arm move.

We have a series of XYZ coordinates in 3D space which we want to send the arm to.
We started with sending them, one by one, to the goto() function through your API. While that sent the arm, there was a jittery motion between each call to goto().

We then took a different approach. Let’s say we want the arm to go from point A to point B. We generated a list of 200 coordinates between points A and B, added them into a dictionary, and then used:

smoothed_trajectories1 = cubic_smooth(angles_dictionary, nb_kp=10)
trajectory.TrajectoryPlayer(reachy, smoothed_trajectories1).play(wait=True, fade_in_duration=1)

to make the arm move in a smooth way. The same was done between point B and C, and so on.

This produced a much smoother motion, but there is one problem. Calling reachy.left_arm.inverse_kinematics() (even with a max_iter=10) 200 times took about 20 second. Of course, we cannot afford to invest 20 seconds of computation for every single motion (if we want to send the arm to 100 different XYZ coords then that would take at least 30 minutes of computation!).

Is anyone aware of any other way we can overcome this problem?

Instead of generating many XYZ coordinates between point A and B, we decided to run inverse_kinematics() for only points A and B, get the 8 corresponding motor angles for the 2 points, and then generate many motor angles between them.
This way, we run inverse kinematics only twice for each motion.

1 Like

There is a way to reduce the jittery motion between each goto by adding the option starting_position='goal_position’

def goto(self,
         goal_positions, duration,
         wait=False, interpolation_mode='linear'):
    Goto specified goal positions.
        goal_positions (dict): desired target position (in the form {'full_motor_name': target_position})
        duration (float): move duration (in sec.)
        starting_point (str): register to use to retrieve the starting point (e.g. 'present_postion' or 'goal_position')
        wait (bool): whether or not to wait for the end motion before returning
        interpolation_mode (str): interpolation used for computing the trajectory (e.g. 'linear' or 'minjerk')
        list: list of reachy.trajectory.TrajectoryPlayer (one for each controlled motor)

This should avoid jerky motion when the next goto start. Can you try if it solve your case ?

1 Like