## Description

1. (130 Points) In this homework, you are supposed to create a complete point-to-point reaching behavior for the NAO robot. First, you will need to create a planning system for point-topoint movements for the endeffector (right hand) of the robot. Second, you will need to implement the plan on the robot simulator and visualize the outcome to confirm that it works.

a) For planning, you are to create a simple dynamical planning system:

x! = α

τ

x( f − x)

Assume the movement duration is τ , and the movement starts at t=0. Assume that at start the

movement is at x0 and it is supposed to reach xf after . Use Simulink to simulate this system, by using a “Step Input” from the “Sources” library, a transfer function from the “Continuous” blocks library, and visualize the step input and the output of the transfer function in

one Scope block. Add another Scope output that visualizes x! : connect the output of the

transfer function to a “Derivative” block (“Continuous” Library), which feeds into another

Scope. Set the simulation parameters to “Fixed-Step” with 0.001s step size. Provide a printout of i) the mathematical analytical form of the transfer function, ii) your Simulink model

(i.e., a picture of the Simulink block diagram), iii) the position and velocity plots from the

Scopes for α = 1,τ = 1, xf = 1, x0 = 0 . Note that is the “final value” parameter in the Step

block.

b) Adjust α such for τ = 1, the movement has reached xf to more than 99% after one second.

Provide a print-out of your choice of α and the position and velocity plot (see above) for this

choice. For your choice of α , also provide the same plots for τ = 2,τ = 5,τ = 10 .

c) Provide two pros and two cons about this trajectory planning system.

d) Write a Matlab program that implements this planning system with Euler integration, again

using an integration step size of 0.001s — essentially, you implement now what Simulink did

for you before. This program should have a subroutine [x!n+1, xn+1 ] = f xf , x ( n ) which computes the next desired state x!n+1, xn+1 based on the current state and goal. This subroutine

needs to integrate the current state xn with the computed next velocity to the next state. Store

all states x!, x along the trajectory in vectors or a matrix, and plot the variables at the end of

your program. Provide a print-out of your program and these plots. Compare against 1a) and

comment on potential differences of your results.

e) A nicer version of dynamical system for planning would be the following:

!x! = α

τ

β x ( ( f − x) − x!)

τ

xf

2

with α = 25,β = 6 . Adjust your matlab program from d) to use this dynamical system as basis of planning. Note that you have a 2nd order system now which needs to generate position,

velocity, and acceleration at the next time step. Create the same plots as in d) for a movement

with τ = 1. Comment on the differences, pros and cons of this new planning system.

f) Another favorable form of creating moving plans is a 5th order spline (minimum jerk spline):

x(t) = c0 + c1t + c2t

2 + c3t

3 + c4t

4 + c5t

5

Assume the movement duration is , and the movement starts at t=0. Assume that at start the

movement is at , and it is supposed to reach at time t= . Determine all the constants

c0 to c5 as a function of , , and their derivatives, and . At start and end velocities and

accelerations are x!(0) = x!0 , x!(τ ) = x!f ,!x!(0) = !x!

0 ,!x!(τ ) = !x!f .

g) A useful way to implement the min jerk spline planning system is by creating a function that

takes as input variables the current state x(t), x!(t),!x!(t), the remaining time to go τ togo , and

the target state xf , x!f ,!x!f . The output of the function would be x(t + Δt), x!(t + Δt),!x!(t + Δt),

i.e., the planned state one time increment ahead. This function can be used to plan the next

desired state given the current desired state, which is useful in a control loop of a robot. Note

that the time-to-go τ togo needs to be decreased at every iteration of the control loop by Δt .

Implement this function in matlab, and create a movement with the same start, goal, and duration as in e), and provide the same plots of the trajectory. Compare the results of the min.

jerk movement with the dynamic system from e) and comment on the differences and similarities.

h) The webpage has a file min_jerk_spline_task.cpp. Modify this file to implement your min

jerk spline function from matlab. Run the task using the “setTask” command in the blue

task_servo window. You will automatically collect a data file, and you should save it to disk

with “saveData”. Visualize the trajectories of all 5 right arm DOFs in CLMCPLOT in

matlab, i.e., print the data traces R_SFE_th and R_SFE_des_th in the top most chart of clmcplot, and then the data traces for R_SAA, R_HR, R_EB, and R_WR in the next 4 charts.

Create a print-out of your visualization and include it in your homework. Comment on the

quality of tracking for the 5 DOFs. Repeat the same for the plot, this time using joint velocities, and then another plot using joint accelerations.

i) Add three additional targets to you C-program such that the left arm creates approximately a

square in Cartesian space. Note that due to the joint-space movements, the hand movement in

Cartesian space is curved. Your square should end at the same point where it started. Collect

data of your movement and save it such that it can be visualized with CLMCPLOT. In clmcplot, click the “PhasePlot” button, and then click LEFT_HAND_x and then

LEFT_HAND_y. This creates a window that plot LEFT_HAND_x in the horizontal axis, and

LEFT _HAND_y in the vertical axis. Provide this plot in your homework and comment on

what is good or bad about your realization of the square. Repeat this plot for

LEFT_HAND_x and LEFT_HAND_z and provide this print-out as well. Comment on the

quality of your square from this view.

j) Modify your C-code from h) and i) to work with the dynamic system planner from e) and

provide the same results as in h) and i) for this planning method (make your C-code such that

τ

x0 xf τ

x0 xf τ

3

it can switch between the two planning methods). Comment on the differences in performance from a technical point of view, and also from the point of view how movements look

like in the simulator.