About Two Motion Planning Papers

Terry Taewoong Um (terry.t.um@gmail.com)
University of Waterloo
Department of Electrical & Computer Engineering
Terry Taewoong Um
MOTION PLANNING
1

MY PREVIOUS RESEARCH
2
• 2008-2010 RRT planning
• 2011-2012 Exoskeleton

CONTENTS
1. Preliminaries
2. Dynamically-Stable Motion Planning for
Humanoid Robots (2002)
3. Whole-body Motion Planning with Centroidal
Dynamics and Full Kinematics (2014)
3

CONTENTS
4
1. Preliminaries

5
WHY DO WE NEED PLANNING?
• What would be most important in planning?
① Optimality
② Stability
③ Completeness

6
COMPLETENESS
• If there are solutions for a problem, a planning algorithm can find a solution. If
there is no solution for a problem, a planning algorithm return failure.
→ We can say “This planning algorithm is complete”
• Probabilistic completeness
: With enough samples, the probability that it
finds an existing solution converges to one.
• Most sampling-based motion planning algorithm
aims at guaranteeing probabilistic completeness
(One of the easiest way is using randomness)

7
HANDLING CONSTRAINTS
• Nonholonomic constaints • Constrained manifold

8
TANGENT SPACE RRT (TS-RRT)

9
QUESTIONS: CONSTRAINTS
Q) Would it be beneficial for the planning software to compute
more than just the first solution it finds?
Q) How do we make sure there is no conflict between constraints
or having conflict is not important here?
Q) Perhaps some constraints are more important than others. Is
there any benefit to assign priority to the constraints here? If yes,
how can we implement a hierarchy for constraints (like the one
we learned in full body control paper)?
Q) What is the secret that the algorithm worked well in such hard
tasks? Say for example in monkey bar task, any small deviation
from desired posture or torque will lead to failure.

CONTENTS
10
2. Dynamically-Stable Motion Planning
for Humanoid Robots (2002)

11
OVERVIEW
• First phase :
statistically-stable, collision-free path
• Second phase :
smooths and transforms this path into
dynamically-stable trajectory
• Given
- Environment Model (collision checking is possible)
- Initial / Goal posture (collision-free & statistically-stable)
- Support base : The location of the supporting foot
• Robot model :
Q) Why do we need both P and C representations?
Q) How would this be
used with support foot
movement? Can you
switch feet or take steps?

12
PROBLEM FORMULATION
• Query & Solution :
• Balance & Torque constraints : q is statistically-stable if
i) The projection of X(q) along the gravity vector g lie within the area of
support SP (the convex hull of all points of contact)
ii) The joint torques do not exceed the maximum torque bound
Q) In what situations do we need the second condition for static-stability?
• Obstacle =
• Dynamic stability : statically-stable → slowing down→ dynamically-stable
AutoBalancer – online balance compensation scheme
Q) How can any statically-stable trajectory be transformed into a dynamically-stable
trajectory by arbitrarily slowing down the motion?

13
PATH SEARCH
• There is no method for explicitly representing
• But we have collision checker & stability checker
• Distance metric
- Higher weights to the links with greater mass and proximity to the trunk
Q) What does that mean how mass and proximity to torso relates to similarity in
configurations?
Q) Why in equation (2) did they assign a higher value to joints closer to the torso?
What effect does this have on the motion?
Q) Do you think you can come up with
better distance metric using lie group?

14
RRT
Q) Is it possible for RRT to enter local minimas and get stuck?
Is this when the failure after a preset time limit happens?

15
RRT FOR STABLE MOTIONS
- Difference : instead of
- Reached / Trapped / Advanced
- The tree attempts to make an incremental
motion toward q
If , add q to the tree
Q) Why do we have to pick q among statically stable postures
despite q will not be part of path unless ρ(q, qnear)<ϵ ?
Q) How can we define ϵ?
Q) Can you explain the RRT_CONNECT_STABLE function?

16
RANDOM STATICALLY-STABLE POSTURES
• We require generating random statically-stable postures – not an easy work
• In this paper, N stable samples is generated as a preprocessing step
Q) Does Qstable assume a flat ground?
Q) They mention that it is more time efficient to pre-load stable postures for the specific
robot. Would this only be valid for a known, flat stable base? Would any modification to
the surface the robot was on make this invalid?
• If the planner fails to find a path after all N samples have been removed from the
currently active Qstable set, a new one can be loaded with different samples

17
SINGLE & DUAL-LEG SUPPORT
Q) In finding single leg support stable configurations, if we can use mirroring why do we check for
right leg as support and left leg as support separately?
Q) Why single support configuration is separated form double support one, and how the algorithm
recognized and combines these two configurations to reach the goal?
Q) When we are using both feet we have bigger support polygon so why it is more complicated to
generate statically-stable body configurations supported by both feet?
Q) In finding double support stable configurations, why at each time one leg is kept fix? Can’t we
fix both of the legs and just check the stability criteria?
[Single-Leg]
[Dual-Leg]

18
TRAJECTORY GENERATION
[Smoothing] [Filtering: Autobalancer]
• Enforce constraints upon the center
of gravity projection and ZMP
trajectory in order to maintain
overall dynamic stability
• The output configuration of the
filter is guaranteed to lie in Cstable
• Filter → Collision check → Speed up
Q) What does the path smoothing block do and why is has been separated from filtering block?
Q) We already made sure that every intermediate posture from initial state to goal be stable, so why
filtering is necessary? If we remove Filtering from the algorithm will it still work?
Q) It uses keeping the COM above the SP. Does this not make it difficult to balance when the robot is
in motion since the ZMP is not the COM projection on the ground during movement?
Q) Will this method be able to be used in motion that goes through unstable states?

19
EXPERIMENT
Dynamically-stable
crouching trajectory for
retrieving an object from
beneath an obstacle
H6: 33 DOF, 137cm, 51kg
Dynamically-stable
planned trajectory for a
crouching motion with H5

20
EXPERIMENT
Reaching for an object atop
a cabinet while avoiding
obstacles and balancing on
the right leg.
Positioning the right foot
above an obstacle while
balancing on the left leg

21
EXPERIMENT
• Each of the scenes contains over 9,000 triangle primitives.
Q) What is a triangle primitive? Q) In the case of validation, no
complicated task has been used to
evaluate the algorithm. How do you
think the algorithm respond to a
complicated task and what will be the
computational time considering table 1?
• Limitations
- The ability to change the base of support, or perform “dynamic” transitions such as jumping
or hopping from one foot to the other would be an exciting improvement.
- The effectiveness of different configuration space distance metrics needs to be investigated
- We currently have no method for integrating visual or tactile feedback
Q) How integration of visual or tactile
feedback will improve this method?

CONTENTS
22
3. Whole-body Motion Planning with Centroidal
Dynamics and Full Kinematics (2014)

23
INTRODUCTION
• Robots in DRC are often restricted to the
quasi-static regime
• Trajectory optimization with full-body
dynamics takes excessively long time to run &
may suffer from local minima
• Planning that uses ZMP over-simplifies model
so that it ignores all kinematic constraints +
the COM height should be constant +
centroidal angular momentum is zero.
Q) What are the advantages and disadvantages of using
direct trajectory optimization problem based on the full
body kinematics and centroidal dynamics?
Q) What are the benefits and handicaps using centroidal
dynamics?

24
INTRODUCTION
• Neither of ZMP or CWS/CWC method makes use of a full body model
• Good news : Simple dynamics formulation call be formulated as a nonlinear
trajectory optimization problem
• In this paper, we use a full kinematics model to enforce geometric contact
condition and a dynamic model that encodes the relationship between the
contact wrench on the robot and the robot’s momenta
• Maintaining the contact wrench sum (CWS) within contact wrench cone (CWC)
CWS : the sum of the force/moment applied to the robot
CWC : the set of the CWS spanned by the sum of the friction cone at each contact pt
• “CWS = ZMP”, “CWC = Support polygon” when the robot walks on a flat plane
The contact should be strongly stable if the CWS is an internal element of the
CWC under sufficient friction.
Q) Can you explain the difference between the ZMP and CWS/CWC for defining balance?

25
SIMPLE DYNAMICS MODEL
• The floating base are determined by the
motion of the robot’s links and the contact
wrenches and the gravitational force
• Since many robots have enough actuators and there is always
a way to achieve such motion, the above equations are
necessary and sufficient to describe the dynamics of the robot
Q) Do you think the contact forces are important for resultant
behaviour of the system? (Relationship with centroidal dynamics)

26
FULL KINEMATICS MODEL
• Geometric constraints:
The left foot and the right foot toes are constrained to lie within the
shaded regions. A point (red sphere) on the right hand is constrained to
be within the shaded bounding box. The head camera gazes at the
point (red sphere) on the right hand, such that the point is within a
cone originated from the camera, with 15°being the half angle of the
cone. The left hand orientation is constrained to be the same as the
green hand drawn by side
• We solve the inverse kinematics which requires to determine n+6 decision
variables for each instant as a large nonlinear optimization problem
• Fortunately, each constraint depends on only a small fraction of the decision
variables, thus, tend to be sparse

27
COLLISION MODEL
• Attach Convex collision geometries to each link and
the world.
• Distance from obstacles can be easily calculated
Q) Is collision prevented by ensuring that each point on the
model does not collide with any obstacle? Doesn’t that result
in a very computationally intensive algorithm?
• Collision constraint

28
TRAJECTORY OPTIMIZATION
• Decision variables
robot state q, v, COM position r, r’, r”, contact positions c, contact forces F,
contact torques 𝝉 , centroidal angular momentum h, h’
• Sample all time-varying quantities at N knot points Q) What is a knot point?
posture error joint vel. COM acc.
- prevent a large contact wrench
- evenly distribute contact wrenches
My question)
Does anybody know where
the q_nominal from?

29
TRAJECTORY OPTIMIZATION
• Constraints
- Dynamic constraints - Time integration constraints
- Kinematic constraints
COM func
fwd kin func
- Joint limit / contact constraints
friction cone constraints,
bounded magnitude on the
contact wrench, etc.
• Solve this sparse nonlinear optimization problem by using SNOPT
Q) If N contact points are considered, how would unknown impacts from outside
forces (like the side) be considered?

30
UNSCHEDULED CONTACT SEQUENCE
• Traditional approach : pre-specify a contact mode sequence
• # of contact points ↑ => # of possible sequences ↑
• Complementarity Constraints
normal
contact F
dist. from the
contact pt
When contact forces exist at a certain contact point and the contact torque is always zero
if the normal contact force exists, i.e. , the body is in contact, then the tangential
displacement of the body between the two adjacent knots must to be zero;
if the body moves in the tangential directions, then the normal force has to be zero,
i.e., the body is not in contact

31
UNSCHEDULED CONTACT SEQUENCE
Q) They first put L2 norm then removed it from cost function. Why in the first place
they introduced it?
Q) In case of unscheduled contact sequence, what the solver does is to search over all
possible contact sequences. Does that mean we should define all possible contact
sequences for the algorithm to search among them?
Q) How do defining complementary constraints help to avoid predefining of sequence
modes?
• New objective function
With the extra complementarity constraints and the revised objective function,
the solver can search over all possible contact sequences

32
EXPERIMENT

33
EXPERIMENT
• Jumping (used the fixed contact sequence)
Q) Why in jumping off a box we need to
put kinematic constraint that feet avoid
collision box? Didn’t we already take care
of collision in equation 5?
• Running
Q) When they are discussing running
they say they plan a half stride and
mirror it. But if this was applied to
obstructed environments wouldn't this
not be valid as your strides would need
to change to avoid obstacles?
- stride length / speed : 2m , 2m/s

34
EXPERIMENT
• Monkey bars (w/ and w/o a fixed mode sequence)
• Salmon ladder
• Little dog
Q) Is there any planning algorithm that works for
humanoid robots but is not general? I assume
that due to complexity of humanoids compared
to other robots any planning algorithm for
humanoids can be applied to other robots by
some modifications. If you agree why generality
is highlighted in these papers?
Q) Regarding the validation, how strong do
you think is to evaluate the proposed
method only by referring to robot
snapshots and videos and having not
enough numerical results?

35
QUESTIONS
Q) In what scenarios would this method not work well?
Q) Is this method only implemented in simulation?
Q) Do you think this method will work on an actual atlas robot or is there a
reason they only did this in simulation?
Q) What would be the main challenges in implementing the proposed
method in real ATLAS robot? Do you think it will work at all?
Q) What does it mean that the C++ code is about two orders of magnitude
faster than the MATLAB code for NLPs? Does it mean that Matlab is less
capable to deal with NLPs or is that their coding style results in faster
computation in C++?

36
Thank you
CONTENTS

About Two Motion Planning Papers

More Related Content

Viewers also liked

Similar to About Two Motion Planning Papers

More from Terry Taewoong Um

Recently uploaded

About Two Motion Planning Papers