Inverse dynamics is the problem of computing the joint torques of a robot given its joint positions, velocities and accelerations: \bm{\tau} = \bm{M}(\bm{q})\bm{\ddot{q}}+ \bm{C}(\bm{q},\bm{\dot{q}}) + \bm{g}(\bm{q}).
The goal of this exercise is to understand the different terms in the inverse dynamics problem formulation and to control the robot to reach a desired position using joint space impedance control.
The function inverse_dynamics(q, dq, ddq, param)
implements the inverse dynamics model for a planar manipulator. Its outputs are joint torques.
The function controlCommand(state, param)
takes the current state of the robot [\bm{q}, \bm{\dot{q}}] as input and outputs joint torque commands u
that are sent to the robot.
u
and observe the result.
inverse_dynamics(q, dq, ddq, param)
to compute the gravity compensation torques \bm{g}(\bm{q}). Perturb the robot with the mouse (at the articulations level) and observe the result of the perturbation.
q_target = np.array([np.pi/2, 0])
and stay there with zero velocity dq_target = np.array([0, 0])
. First, use the gravity compensation controller that you coded in the above to bring your robot close to an upright position.
You can then specify the torque control commands that will be sent to the robot so that the resulting closed-loop robot behavior corresponds to a mass-spring-damper system.
You should see that it stabilizes around the upright position. To check how stable your controller is, try to perturb the robot with the mouse.
Modify the values of stiffness and damping to see the effect of perturbations on the impedance controller.