﻿ RCFS packages = ['numpy']

# Exercise 5.bInverse dynamics and impedance control

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.

• You can first run the code with zero torque control commands u and observe the result.
• Specify the control commands so that \bm{\tau} = \bm{g}(\bm{q}) by exploiting the function 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.
• We would now like the robot to reach an upright position 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.

q_target = np.array([np.pi/2, 0]) kP = 30.0 # Stiffness gain kV = 1.0 # Damping gain def controlCommand(state, param): q = state[:param.nbVarX] dq = state[param.nbVarX:] u = np.zeros(param.nbVarX) # Question 1: Gravity compensation term (g(q)) # u = inverse_dynamics(q, dq, ddq, param) # Implement here # Question 2: Joint impedance control for reaching upright position # u = ... # Implement here return u
def controlCommand(state, param): # Question 1: Gravity Compensation Term (g(q)) u = inverse_dynamics(state[:param.nbVarX], np.zeros(param.nbVarX), np.zeros(param.nbVarX), param) return u
q_target = np.array([np.pi/2, 0]) kP = 30.0 # Stiffness gain kV = 1.0 # Damping gain def controlCommand(state, param): # Question 2: Joint impedance control for reaching upright position q = state[:param.nbVarX] dq = state[param.nbVarX:] u = kP * (q_target - q) - kV * dq + inverse_dynamics(q, dq, np.zeros(param.nbVarX), param) return u