Robotic Motion Planning: Configuration Space

1 16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaRobotic Motion Planning: Configuration SpaceRobotics Institute 16-735 ~motionplanningHowie ~choset16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaWhat if the robot is not a point?The Scout should probably not be modeled as a Nor should robots with extended linkages that may contact , Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaWhat is the position of the robot?Expand obstacle(s)Reduce robotnot quite right ..16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaConfiguration Space A key concept for Motion planning is a Configuration : a complete specification of the position of every point in the system A simple example: a robot that translates but does not rotate inthe plane: what is a sufficient representation of its Configuration ? The Space of all configurations is the Configuration spaceor formalism:Lozano-Perez 7916-735, Howie Choset with slides from Hager, Z.

2 Dodds, and Dinesh MochaRobot ManipulatorsWhat are this arm s forward kinematics? L1L2(x,y)yx(How does its position depend on its joint angles?)16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaRobot Manipulators L1L2(x,y)c = cos( ) , s = sin( ) c = cos( ) , s = sin( ) c+= cos( + ) , s+= sin( + ) Keeping it simple yxWhat are this arm s forward kinematics?Find (x,y)in terms of and ..16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaManipulator kinematicsc = cos( ) , s = sin( ) c = cos( ) , s = sin( ) c+= cos( + ) , s+= sin( + ) Keeping it simple L1L2(x,y)yxPositionxL1c L2c+yL1s L2s+=+16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaInverse KinematicsInverse kinematics --finding joint angles from Cartesian coordinates L1L2(x,y)via a geometric or algebraic Given (x,y) and L1 and L2,what are the values of , , ?16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaInverse KinematicsInverse kinematics --finding joint angles from Cartesian coordinates via a geometric or algebraic cos-1 x2 + y2 -L12 -L22 2L1L2= sin-1 L2sin( )x2 + y2+ tan-1(y/x)But it s not usually this (y,x) L1L2(x,y) = 180 - (1,0) = , (-1,0) = , , Howie Choset with slides from Hager, Z.

3 Dodds, and Dinesh MochaPuma Inv. Kinematics%% Solve for theta(1)r=sqrt(Px^2 + Py^2);if (n1 == 1),theta(1)= atan2(Py,Px) + asin(d3/r);elsetheta(1)= atan2(Py,Px) + pi - asin(d3/r);end%% Solve for theta(2)V114= Px*cos(theta(1)) + Py*sin(theta(1));r=sqrt(V114^2 + Pz^2);Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/( *a2*r));theta(2) = atan2(Pz,V114) + n2*Psi;%% Solve for theta(3)num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;d en = cos(theta(2))*Pz - sin(theta(2))*V114;theta(3) = atan2(a3,d4) - atan2(num, den);% Solve for theta(4)V113 = cos(theta(1))*Ax + sin(theta(1))*Ay;V323 = cos(theta(1))*Ay - sin(theta(1))*Ax;V313 = cos(theta(2)+theta(3))*V113 + sin(theta(2)+theta(3))*Az;theta(4) = atan2((n4*V323),(n4*V313));% Solve for theta(5)num = -cos(theta(4))*V313 - V323*sin(theta(4));den = -V113*sin(theta(2)+theta(3)) + Az*cos(theta(2)+theta(3));theta(5) = atan2(num,den);% Solve for theta(6)V112 = cos(theta(1))*Ox + sin(theta(1))*Oy;V132 = sin(theta(1))*Ox - cos(theta(1))*Oy;V312 = V112*cos(theta(2)+theta(3)) + Oz*sin(theta(2)+theta(3));V332 = -V112*sin(theta(2)+theta(3)) + Oz*cos(theta(2)+theta(3));V412 = V312*cos(theta(4)) - V132*sin(theta(4));V432 = V312*sin(theta(4)) + V132*cos(theta(4));num = -V412*cos(theta(5)) - V332*sin(theta(5));den = - V432;theta(6) = atan2(num,den);it s usuall much worse!

4 16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaSome Other Examples of C- Space A rotating bar fixed at a point what is its C- Space ? what is its workspace A rotating bar that translates along the rotation axis what is its C- Space ? what is its workspace A two-link manipulator what is its C- Space ? what is its workspace? Suppose there are joint limits, does this change the C- Space ? The workspace?16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaWhere can we put ? Configuration SpaceAn obstacle in the robot s workspace 2703601809009018013545 Torus(wraps horizontally and vertically)qAqB AB16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaObstacles in C- Space Let qdenote a point in a Configuration Space Q The path planning problem is to find a mapping c:[0,1] Q no Configuration along the path intersects an obstacle Recall a workspace obstacle is WOi A Configuration Space obstacle QOiis the set of configurations qat which the robot intersects WOi, that is QOi= {q Q| R(q) WOi } The free Configuration Space (or just free Space ) QfreeisQfree= Q \ ( QOi)The free Space is generally an open setA free pathis a mapping c:[0,1] QfreeA semifree pathis a mapping c:[0,1] cl(Qfree)16-735, Howie Choset with slides from Hager, Z.

5 Dodds, and Dinesh MochaDisc in 2-D workspaceworkspaceconfiguration spaceworkspace16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaExample of a World (and Robot)ObstaclesFree SpaceRobotx,y16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaConfiguration Space : Accommodate Robot SizeObstaclesFree SpaceRobot(treat as point object)x,y16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaTrace Boundary of WorkspacePick a reference , Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaPolygonal robot translating in 2-D workspaceworkspaceconfiguration space16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaPolygonal robot translating & rotating in 2-D workspaceworkspaceconfiguration space16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaAny reference pointxyP45 degreesR16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaAny reference point configurationTaking the cross section of Configuration Space in which the robot is rotated 45 degreesHow many sides does P R have?

6 PR16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaAny reference point configurationTaking the cross section of Configuration Space in which the robot is rotated 45 degreesHow many sides does P R have?PR16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaMinkowski sum The Minkowski sumof two sets Pand Q, denoted by P Q, is defined asP+Q= { p+q| p P, q Q} Similarly, the Minkowski differenceis defined asP Q= { p q | p P, q Q}pq16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaMinkowski sum of convex polygons The Minkowski sum of two convex polygons Pand Q of mand nvertices respectively is a convex polygon P + Qof m + nvertices. The vertices of P + Q are the sums of vertices of P , Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaObservation If Pis an obstacle in the workspace and Mis a moving object. Then the C- Space obstacle corresponding to P is P , Howie Choset with slides from Hager, Z.

7 Dodds, and Dinesh MochaStar Algorithm: Polygonal Obstaclesr1r2r3e1e2e3e4e3e1e2e4r2r1r316- 735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaStar Algorithme1e2e3e4r1r2r3e3e1e2e4r2r1r316- 735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaStart Point Leave that as an exercise for your , Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaWhere do we put ? Configuration Space Quiz An obstacle in the robot s workspace 2703601809009018013545qA qBABT orus(wraps horizontally and vertically)16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaConfiguration Space ObstacleAn obstacle in the robot s workspace 2703601809009018013545qBqAThe C- Space representation of this How do we get from Ato B?ABReference configuration16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaTwo Link PathThanks to Ken Goldberg16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaTwo Link Path16-735, Howie Choset with slides from Hager, Z.

8 Dodds, and Dinesh MochaProperties of Obstacles in C- Space If the robot and WOiare _____, then Convex then QOiis convex Closed then QOiis closed Compact then QOiis compact Algebraic then QOiis algebraic Connected then QOiis connected16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaAdditional dimensionsWhat would the Configuration Space of a rectangular robot (red) in this world look like? Assume it can translate androtate in the plane.(The blue rectangle is an obstacle.)xy16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh Mochaa 2d possibilitywhy not keep it this simple?xy2d , Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaA problem?qinitqgoalwith otherwise straightforward ~sethian/Applets/java_files_robotic_ , Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaRequires one more conservative !what instead?16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaWhen the robot is at one orientationqinitqgoal0 it can make , Howie Choset with slides from Hager, Z.

9 Dodds, and Dinesh MochaqinitqgoalWhen the robot is at another orientationit , Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaAdditional dimensionsWhat would the Configuration Space of a rectangular robot (red) in this world look like?(The obstacle is blue.)xy0 90 180 this is , Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaPolygonal robot translating & rotating in 2-D workspacexy 16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaSE(2)16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh Mocha2D Rigid Object16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaThe Configuration Space (C- Space ) 1 2 3CC--spacespaceworkspaceworkspaceTOPTOPV IEWVIEW 1 2 3 1 2 316-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaMoving a Piano16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaConfiguration Space (C- Space )q2q1q3q0qnq4[0, ]t INIT:INIT:(0)QGOAL:GOAL:(T)Q =)()()(0tqtqtQnM(t)Q16-735, Howie Choset with slides from Hager, Z.

10 Dodds, and Dinesh MochaTopology?2R manipulatorConfiguration spaceSphere?Torus?16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaWhy study the Topology Extend results from one Space to another: spheres to stars Impact the representation Know where you are Others?16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaThe Topology of Configuration Space Topology is the intrinsic character of a Space Two Space have a different topology if cutting and pasting is required to make them the same ( a sheet of paper vs. a mobius strip) think of rubber figures --- if we can stretch and reshape continuously without tearing, one into the other, they have the same topology A basic mathematical mechanism for talking about topology is thehomeomorphism. 16-735, Howie Choset with slides from Hager, Z. Dodds, and Dinesh MochaHomeo- and Diffeomorphisms Recall mappings: : S T If each elements of goes to a unique T, is injective(or 1-1) If each element of T has a corresponding preimage in S, then is surjective(or onto).