Modular Pick and Place Simulator using ROS Framework
main
1. Project and Implementation of a Dynamically
Stable Gait in the Robot Nao
Universit`a degli Studi di Roma “La Sapienza”
Master in Mechanical Engineering
Rafael Rojas
Rafael Rojas Biped Locomotion in Nao Robot
2. 1 Motivations.
2 Kajita simplification of gait dynamics.
3 Walking Pattern Generator.
4 The kinematic controller.
5 Implementation in Nao.
Rafael Rojas Biped Locomotion in Nao Robot
3. Motivations
Nao comes with build-in functions to make the robot walk
given some parameters, such velocity or a position of arriving.
But we don’t have access to the walking algorithm.
To modify the motion planner and make it robust, for
example, with respect to external disturbance, it is necessary
to have access to the walking pattern generator.
The development of a new walking pattern generator it is
necessary also for planning the motion of the robot through
obstacles.
Rafael Rojas Biped Locomotion in Nao Robot
4. The Kajita simplified dynamics
zc
g
¨x + x = ZMPx
zc
g
¨y + y = ZMPy (1)
This approach have a linear relationship between the state of the
system and the Zero Moment Point (ZMP) position in the
locomotion plane.
Rafael Rojas Biped Locomotion in Nao Robot
5. WPG
The Walking Pattern Generator (WPG) is a block, where a
footstep sequence with a ZMP trajectory are transformed into a
trajectories of the feed and the center of Mass (CoM) that allows
an humanoid robot to walk with dynamic stability.
Rafael Rojas Biped Locomotion in Nao Robot
6. WPG: Preview Controller
Our WPG is based on a feedback controlled system with a control
law noted as preview controller.
Rafael Rojas Biped Locomotion in Nao Robot
7. WPG: Preview Controller
The control law is:
u(k) = −GI
k
i=0
e(i) − Gx x(k) −
NL
l=1
Gd (l) ZMPd (k + l)
and the values of GI , Gx and Gd (l) are calculated to minimize the
performance index:
J =
∞
i=k
eT
(i)Qee(i) + ∆xT
(i)Qx ∆x(i) + ∆uT
(i)R∆u(i)
this is done through the resolution of the discrete algebraic riccati
equation (DARE).
Rafael Rojas Biped Locomotion in Nao Robot
8. WPG: our variables
Our WPG have the follow variables that we can modify:
time of double support Tds
time of single support Tss
discretization time T
height of the car of the kajita model zc
the trajectory of the swinging foot
the trajectory of the ZMP
Rafael Rojas Biped Locomotion in Nao Robot
9. DARE risolution
with a discretization time of T = 0.01 s, Nl = 150 future samples
of ZMPd and zc = 0.31
0
500
1000
1500
2000
0 0.2 0.4 0.6 0.8 1 1.2
GI = 1149.635782.
Gx = 42986.459193 8251.545731 114.108039.
Gd is in the figure.
Rafael Rojas Biped Locomotion in Nao Robot
14. WPG: trajectory in the space
0
0.1
0.2
0.3
0.4
0.5
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.20
0.1
0.2
0.3
0.4
0.5
Rafael Rojas Biped Locomotion in Nao Robot
15. Kinematic Controller
the WPG implicitly splits the problem of the dynamically stable
gait in two parts:
a dynamic part solved by the WPG, that generates stable
trajectories
a kinematic part of solving the inverse kinematic problem of
tracking the trajectories generated by the WPG.
Rafael Rojas Biped Locomotion in Nao Robot
16. Kinematic Control: resolution of kinematic redundancy
the general solution of the inverse kinematics
for a task ti is:
˙q = J#
ti
˙ti +Pi uj where Pi = I−J#
ti Jti and uj ∈ R
the vector uj ∈ Rn can be used to track
another task tj :
uj = (Jtj Pi )#
tj − Jtj J#
ti
˙ti + Pj uk
with Pj = I − (Jtj Pi )#
(Jtj Pi )
Rafael Rojas Biped Locomotion in Nao Robot
17. this reasoning can be used
recursively to solve and array of N
task t1, t2, · · · , tN sorted by priority,
defining:
Ωi = Ωi−1Pi with Ω0 = I
˜Jti = Jti Ωi−1 with ˜Jt1 = Jt1
Pi = I − ˜J#
ti
˜Jti ,
vi =
i
k=1 Ωk−1
˜J#
tk
˙tk − Jkvk−1
with v0 = 0.
˙q =
N
i=1
Ωi−1
˜J#
ti
˙ti − Ji vi−1
after, we need to integrate numerically: qk+1 = qk + ˙q(k)∆T
Rafael Rojas Biped Locomotion in Nao Robot
18. General View of the Project
Rafael Rojas Biped Locomotion in Nao Robot
19. Simulation in Webots
Webots is an environment of develop, design and dynamic
simulation of mobile robots.
Rafael Rojas Biped Locomotion in Nao Robot
20. Simulation: Indici di prestazioni
Now we can show an example of the performance of our WPG and
kinematic controller on the physic model of Nao that we can found
in webots.
The performance index that we have used is:
Total Square Torque: TST = QT Q,
where Q is the vector of actual torque in the motors.
Rafael Rojas Biped Locomotion in Nao Robot
25. Integration in Nao
We start the implementation of our software in NaoQi from one
module example named fastgetsetexample that uses the
module DCM. We have modified the lines from 347 to 385 and
there we have put the next pseudocode :
1 get DCM time and set in the variable command
2 get the feedback of the motors of nao with
fMemoryFastAccess->GetValues(sensorValues);
3 compute the WPG actual desired position and velocities
4 resolve the inverse kinematic problem as we shown below
5 send the desired position to Nao with
dcmProxy->setAlias(commands);
Rafael Rojas Biped Locomotion in Nao Robot