Project 2: Potential Field Planning for Rigid Bodies and Manipulator Chains
[support code in C++ and Matlab]
 Implement potential field planners
in the case of rigid bodies and manipulator chains
 Your implementation should also contain strategies for escaping
local minima when the robot gets stuck. One possibility is to
considerably increase the repulsive potential in order to push the robot away
from the obstacle. Another one is to move in a random direction
away from the obstacle. You could also try adding other artificial
goal/obstacle points to guide the robot away from the real
obstacles and toward the real goal.
 You can work by yourself or with a partner of your own choice
 Implementation can be done in C/C++ or MATLAB on top of the provided support code
Environment consists of a robot, a goal region represented as a
circle, and several obstacles represented as circles as
well. Graphical interface supports the dynamic addition and
displacement of obstacles and goal. A new obstacle can be added at any
time by pressing down the left button of the mouse. Each obstacle and
the goal can be moved around by clicking inside them and holding the
left button down while moving the mouse. To run the planner, user
needs to press the key 'p' on the keyboard. Pressing 'p' again toggles
between running and stopping the planner. User can change the radius
of each obstacle and the goal by first pressing the key 'r' and then
clicking inside an obstacle/goal, holding the leftbutton down while
moving the mouse. Pressing 'r' again toggles between editing radius
or moving the obstacles and the goal.
Implementation Notes: Rigid Body
Simulator provides access to goal center, goal radius, number of
obstacles, current robot configuration (robot x, robot y, robot
theta), and vertices of the polygon representing the robot.
Simulator also computes the closest point on the ith obstacle for any
given point (x, y), see function ClosestPointOnObstacle
When computing the potential, it is recommended that you use each
vertex of the polygon robot as a control point.

For each control
point (xj, yj), you should then
 compute its Jacobian matrix Jac_{j}
 compute the workspace gradient (wsg_{i}) to each obstacle
[In my implementation, wsg_{i} is zero if the
distance from (xj, yj) to the closest point on the ith obstacle is larger than a
small constant. I then scale the gradient in order to ensure that
the robot gets pushed away when coming
close to the obstacles. You may need to play around in order to
figure out by how much to scale the gradient in order to get the
desired behavior.]
 use the Jacobian matrix to transform wsg_{i} into a
configuration space gradient csg_{i}, which is then added to
the overall configuration space gradient csg
 compute the workspace gradient to the goal, scale it appropriately,
transform it into a configuration space gradient via the Jacobian,
and add it to the overall configuration space gradient
 Now that the overall configuration space gradient has been
computed, scale it so that you make a small move in x and y and a
small move in theta. Recall that the robot should always move in
direction opposite to the gradient.
If using C++ support code, you should implement the
function ConfigurationMove in the
class RigidBodyPlanner. This function should
return RigidBodyMove, which represents a small move (dx, dy,
dtheta) by the robot.
If using Matlab support code, you should implement the
function RigidBodyPlanner.
Implementation Notes: Manipulator Chain
Environment consists of a 2d manipulator chain, a goal region
represented as a circle, and several obstacles represented as circles
as well.
Simulator provides access to goal center, goal radius, number of
obstacles, and start and end positions of each link in the chain.
Simulator also computes the closest point on the ith obstacle for any
given point (x, y), see function ClosestPointOnObstacle
Moreover, simulator also implements Forward Kinematics, see
function FK.
When computing the potential, it is recommended that you use the end
position of each chain link as a control point. You should follow a
similar approach as in the case of rigid bodies. One difference is
that in the case of the manipulator chain, the attractive potential
should be computed only between the end effector (last point in the
chain) and the goal and not between any intermediate links and the
goal. In this way, the potential will guide the end effector and not
the intermediate links toward the goal. The repulsive potential, as in
the case of rigid bodies, should be computed between the end of each
link and each obstacle.
Important: You can use an equivalent but easier to compute
definition of the Jacobian. In this definition the partial derivative
of the forward kinematics of the end position of the jth joint with
respect to the ith joint angle with i <= j is given as
 x = LinkEndY(j) + LinkStartY(i)
 y = LinkEndX(j)  LinkStartX(i)
If using C++ support code, you should implement the
function ConfigurationMove in the
class ManipPlanner. This function should compute small moves
for each link angle.
If using Matlab support code, you should implement the
function ManipPlanner.
Support Code: C++
 Support code can be found in CppRigidBodyPFP and CppManipPFP
 To compile the code inside each directory use similar steps as in
the case of the bug algorithms in project 1
 The rigid body program can be run on Windows as
bin\Debug\Planner.exe robotL.txt and on Linux/MacOS as
bin/Planner robotL.txt
where the argument is the name of the robot file. You can use one of
the provided files or create your own.
 The manipulator chain program can be run on Windows as
bin\Debug\Planner.exe nrLinks linkLength and on Linux/MacOS as
bin/Planner nrLinks linkLength
where nrLinks is a positive integer representing the number of
links and linkLength is a positive real representing the length
of each link.
Support Code: MATLAB
 Support code can be found in MatlabRigidBodyPFP and MatlabManipPFP
 The rigid body program can be run as
RunRigidBodyPlanner('robotL.txt')
where the argument is the name of the robot file. You can use one of
the provided files or create your own.
 The manipulator chain program can be run as
RunManipPlanner(nrLinks, linkLength)
where nrLinks is a positive integer representing the number of
links and linkLength is a positive real representing the length
of each link.