Download 1. What happens with an incorrect model?

Lecture # 20 : 3rd April 2013
Robot Autonomy (16-662, S13)
Perception for Manipulation—World Models
Lecture by: Siddhartha Srinivasa
Scribe: Jiangxia Shi
In the last lecture we talked about the different sensors on HERB that help build the world models, which
present how the robot interprets the real world.
1. What happens with an incorrect model?
When the world model is incorrect, i.e., the simulated world is not equal to the real world, out of possible
reasons like sensor failure to recognize objects in the real world, there are two cases in which things go
wrong when HERB is moving:
CASE 1: The real robot gets in collision. (This refers to the situation where the real world robot collides
with itself or other objects.)
CASE 2: The simulated robot gets in collision. (This refers to the situation where there is no collision in
the real world, but due to reasons like misdetection of an object, the simulated robot gets into collision and
remains stuck since it is unable to plan forward as it fears that further movement would result in
undesirable circumstances.)
2. Detecting the condition & Recovery strategy
2.1 Detecting the condition
Why can HERB fall into the foregoing issues? Incorrect localization could be a result of an unmodeled
obstacle or a poorly detected one; for example, the stretch of the cables which drive HERB’s arms may
increase arm localization uncertainty, or model uncertainty might exist, which means you may have not
modeled the robot and environment precisely.
So how do we detect the condition for the above two cases?
CASE 1: When the real robot is in collision, the simulated robot is always - not in collision; because the
simulated robot has done collision check while planning and will not be aware of the collision. So, the way
to detect that the arms have stalled in the real robot is to use Proportional – Derivative (PD) controllers to
generate feedback, as seen in Figure 1.
Page 1
Figure 1: PD control to detect collision of the robot in the real world
The ‘robot model’ takes the desired velocity & acceleration as an input and outputs the desired actual
torques for each joint. After implementation in the real world, we can get the actual velocity and
acceleration. To conduct PD control, the robot compares the desired values and the real values and uses
weighted results to get back to the correct configuration.
PD control is often used instead of PID (Proportional-Integral-Derivative) control as the robot is barely
responsive and PID control will further slow it down.
Ideally, we expect:
e = 0, 𝜏𝑓 = 0
But uncertainty always exists. For example, most of the robot’s tasks involve picking up something; during
the procedure of picking-up an object, the inertia, etc. of the object may change, thus creating a disturbance
force. Assume you have water in a container and the robot is trying to pick it up: the water may move
while the robot is picking the container; this will confuse the robot about the mass & inertia of the
container; here e and 𝜏𝑓 will be non-zero. In addition, when the robot underestimates some parameter of
the object it wants to pick—mass, for example—the robot may experience a rough / jerky motion.
When e and 𝜏𝑓 are not 0, which means that the real robot might be in collision, the robot needs to know if
something serious is going on. One thing we can do is to set a threshold value δ for the feedback term.
When 𝜏𝑓 < 𝛿, the robot will interpret it as a normal situation, for instance, the object being picked is a
little heavy. When 𝜏𝑓 > 𝛿, the robot will know that something is wrong and declare STALL.
CASE 2: Consider the case when the simulator itself detects collision of the robot. To verify whether the
real robot will collide or not, we can check the feedback torque to see if it is large enough or not. In the
simulator if an object is penetrating the arm and the feedback torque shows that there is no penetration, that
object might be a fake simulated object; if the feedback torque is large enough, the robot will know that
there is a real object in its way.
Page 2
We can get more information from 𝜏𝑓 . As seen in Figure 2, in the real world some part of the robot collides
with an obstacle. But in the simulated world, there is no such obstacle obstructing the robot’s motion.
Figure 2: Real robot is in collision but simulated robot is not.
Assume that the obstacle is in 𝑅 3. How can we localize it?
We know the following:
- Configuration q,
- 𝜏𝑓 ,
- Geometry of the arm.
Assume some location x, we can do some static analysis:
𝐽𝑞𝑇 (𝑥)𝑓(𝑥) = 𝜏𝑓 ,
in which 𝐽𝑞𝑇 (𝑥) and 𝜏𝑓 are known and 𝑓(𝑥) is unknown.
What we can do is:
1) Sample x on the body of the arm.
2) Compute 𝐽𝑞 (𝑥).
3) Compute 𝑓̂(𝑥) = 𝐽𝑞+ 𝜏𝑓 .
4)
5)
Compute C(x) = C(x, 𝑓̂, 𝑔𝑒𝑜𝑚𝑒𝑡𝑟𝑦).
x* = argmin𝑥∈𝑏𝑜𝑑𝑦 𝐶(𝑥) = ∑ 𝛼𝑖 𝐶𝑖 (𝑥).
Cost function can be in three forms:
1) 𝐶1 (x) = sinθ, which looks for the force that aligns with the contact normal.
2) 𝐶2 (x) = ||𝑓̂||, for a certain situation, the absolute value of the feedback force can be 50mN or 50N, so
the force is most likely 50N.
3) 𝐶3 (x) = d(x), which means the distance to the nearest obstacle. If a point on the body of the robot is
the nearest to the obstacle, that point might be the one we are looking for.
But using only one of the cost functions may not be precise. If we are only using the absolute value of a
torque, with half of the moment arm and twice the absolute value of the force, we get the same absolute
value of the torque, but the force is obviously not the same. So to eliminate that, we can combine different
kinds of cost functions and add weights to each cost function term.
Page 3
2.2 Recovery strategy
We have now detected the errors, what do we do to make the robot recover from the situation?
1)
Do nothing. The robot just needs to back-up and try again.
Advantage: It’s good if the robot gets a false positive.
Disadvantage: The robot might keep trying over and over again and it might collide again.
2) Add a work space obstacle.
Advantage: The robot won’t collide again.
Disadvantage: The obstacle added might be wrong.
3) Add a C-space obstacle.
Advantage: It will definitely not be wrong.
Disadvantage: Estimation may be made poorly or incorrectly. The C-space obstacle can limit the
movement of all the joints of that specific configuration, which is not necessarily needed.
One of the things to do is to look at the collision via sensors. Tactile sensors can be used, but sensors can
lead to resolution and mechanical issues. Also, the part of the body which does not have a sensor attached
still cannot be “seen” by the robot. If we use LIDAR, the drawback would be that it won’t work properly if
it is dark, or if the robot arm moves into some cupboard. Some robots are prepared to work in a cluttered
space, so tactile sensors in this sense might be a better option.
3. Techniques for building world models — Occupancy Grid Maps
Input: LIDAR data.
Output: Map of the world.
If a LIDAR is spinning to collect data, as seen in Figure 3, we can get the points at which the laser hits the
obstacle. For points like A, we can infer that the space is empty; for points like B, we can infer that this
space is occupied; for points like C, it is an unknown area.
Figure 3: Laser ‘hits’ of a LIDAR
Page 4
How this is done (as shown in Figure 4):
1) Grid up the world. The degree of dispersion depends on geometry and the resolution of the sensors.
2) Ray trace each laser.
3) Keep two counts: hit and pass-through; we keep the count of passthroughs at the left top corner of
each grid, and the count of hits at the right bottom corner of each grid.
Figure 4: Gridding up the world
As seen in Figure 5, there could be four types of boxes (grids).
Figure 5: Four types of counted grids
But, there might be some issues with this nomenclature created by:
1) Moving obstacles.
2) Sensor models.
3) Adding prior knowledge, i.e., initializing the rays with some amount of the hits and passthroughs.
Page 5