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.
