PD- Control of a robot

In this section you will control the paddle to move to a desired location. The robot is force controlled. This means that for every time step, you can specify an applied force to the robot's center of mass. Additionally you can specify the an applied angular torque.

The goal is to program the robot to move to a desired location specified by $\vec{x}^* = (x,y,\theta)$ by specifing the velocity at each time step.

We will break this into a few steps.

Running the simulation and accessing the robot state information
Open loop control of the robot
Feedback control of the robot

The following code shows the instructor solution for a simple PD controller. You can modify the initial position and desired position/velocity of the robot to see how it works.



In [ ]:

    
import tutorial; from tutorial import *


initial_pose = (16, 12,0.0)
desired_pose = (16, 16,3.14/2.)
desired_vel = (0, 0, 0)

play_pd_control_solution(initial_pose, \
            desired_pose, desired_vel)

PD Control Part 1: Running the simulation and accessing the robot state information

The following code will show a simple example of running the simulator

You should see the robot (paddle) start in the middle of the screen and fall down due to gravity.

Try changing the robot's orientation and rerun the simulation.



In [ ]:

    
import tutorial; from tutorial import *

initial_pose = (16, 12, 3.14/2.)
result = run_pd_control(initial_pose, controller=None)

Let's rerun our simulation and plot the state of the robot over time.



In [ ]:

    
import tutorial; reload(tutorial); from tutorial import *
    
initial_pose = (16, 12, 3.14/2.)
result = run_pd_control(initial_pose, controller=None)
plot(result, "Robot")

PD Control Part 2:Open loop control of the robot

Now we are going to move our robot using open loop control. We can apply a force to the center of mass in the x or y direction, and an angular torque about the center of mass.

One of the inputs to the run_pd_control is currently set to None. In this example we are going to show how to write a controller that gets run at every time step.

The output of the controller is $u_x, u_y, u_th$, which is the amount of force applied in the x direction, in the y direction, and angular torque applied. The force is applied to the robot's center of mass.



In [ ]:

    
import tutorial; reload(tutorial); from tutorial import *
    
initial_pose = (16, 12, 3.14/2.)

def openLoopController (time, robot_state):
    u_x = 1.0
    u_y = 0
    u_th = 0
    return u_x, u_y, u_th

result = run_pd_control(initial_pose, openLoopController)
plot(result, "Robot")

Suppose we want to move our robot up 4 meters to position (16, 16) from position (16, 12) using our open loop control function. What forces should we apply and for how long? The mass of the robot is 2 kg.

Assuming we apply a constant force $u_y$, the dynamics of the system will be: $$ y(t) = y_0 + \frac{1}{2}(\frac{u_y}{m} - 9.81)t^2 $$

If we assume the force will be applied for 2 seconds only, we can find what constant force to apply:

$$ 16 = 12 + \frac{1}{2}(\frac{u_y}{m} - 9.81)2^2 $$$$ u_y = 23.62 $$

Program the robot to move to position (16, 15) using open loop commands. How close can you get?



In [ ]:

    
import tutorial; reload(tutorial); from tutorial import *
    
initial_pose = (16, 12,0.0)


constant_force = 23.62
time_applied = 2

def openLoopController (time, robot_state):
    
    u_x = 0
    u_y = 0
    u_th = 0
    
    # only apply force for time < time_applied
    if time < time_applied:
        u_y = constant_force
        
    # when the robot is near time_applied print the current y value
    if abs(time-time_applied) < 0.1:
        print "Time: %.2f, Height: %.2f " % (time,  robot_state[1])
    return u_x, u_y, u_th

result = run_pd_control(initial_pose, openLoopController)
plot(result, "Robot")

PD Control Part 3: Feedback control of the robot

The open loop controller method we used required a lot of effort on the designers part and won't work very well in practice. In this case we knew the robot's mass and could perfectly apply a force in the center of motion.

An alternative method is to use the current state of the robot to determine what force to apply. In this next section you are going to implement a position controller.

The following is an equation for a position controller:

$$u = K_{p}\cdot(X_{desired} - X_{current})$$

$u$ is the output of our controller
$K_{p}$ is the proportional gain
$X_{desired}$ is the reference signal
$X_{current}$ is the output signal
$(X_{desired} - X_{current})$ is the error signal

This controller is going to apply forces in the direction that decreases the error signal.

The robot state is given to you as $(x, y, \theta, \dot{x}, \dot{y}, \dot{th})$.



In [ ]:

    
import tutorial; reload(tutorial); from tutorial import *
    

initial_pose = (16, 12,0.0)
desired_pose = (16, 16,0.0)

K_px = 10
K_py = 10
K_pth = 10


def closedLoopController (time, robot_state):
    
    # the output signal
    x,y,th, xdot, ydot, thdot = robot_state
    
    # the reference signal
    rx, ry, rth = desired_pose 
    
    # the error signal
    e_x = rx - x
    e_y = ry - y
    e_th = rth - th
    
    # the controller output
    u_x = K_px*e_x
    u_y = K_py*e_y
    u_th = K_pth*e_th
    
    return u_x, u_y, u_th 

result = run_pd_control(initial_pose, closedLoopController)
plot(result, "Robot")

PD Control Part 3: Feedback control of the robot (continued)

Activities:

Try using different gains. See if you can observe different system response behavior, such as:
- under damped
- damped
- overdamped
Improve upon your controller by adding a derivative term. In this case the reference signal for the derivative terms should be equal to 0.

$$u = K_{pose}\cdot(X_{desired} - X_{current}) + K_{d}\cdot(\dot{X}_{desired} - \dot{X}_{current})$$

$u$ is the output of our controller
$K_{d}$ is the derivitave gain
$\dot{X}_{desired}$ is the reference signal (In our case it is equal to 0)

`rxdot, rydot, rthdot = 0,0,0 `



In [ ]:

    
import tutorial; reload(tutorial); from tutorial import *
    

initial_pose = (16, 12,3.14/2)
desired_pose = (3, 16,0.0)
desired_vel = (0, 0, 0)

K_px = 100
K_py = 100
K_pth = 10
K_dx = 50
K_dy = 50
K_dth = 20

def closedLoopController (time, robot_state):
    
    # the output signal
    x,y,th, xdot, ydot, thdot = robot_state
    
    # the reference signal
    rx, ry, rth = desired_pose 
    rxdot, rydot, rthdot = desired_vel
    
    # the error signal
    e_x = rx - x
    e_y = ry - y
    e_th = rth - th

    e_xdot = rxdot - xdot
    e_ydot = rydot - ydot
    e_thdot = rthdot - thdot
    
    
    # the controller output
    u_x = K_px*e_x + K_dx*e_xdot
    u_y = K_py*e_y + K_dy*e_ydot
    u_th = K_pth*e_th + K_dth*e_thdot
    
    return u_x, u_y, u_th 

result = run_pd_control(initial_pose, closedLoopController)
plot(result, "Robot")