Ellipsoid of Mobility

Bob Beretta
10/21/97

Discussion

Consider a two-link spatial robotic arm with three degrees of freedom at the end of the terminal link.  If a unit force is applied to the end of the terminal link, the resulting acceleration of the link will vary in magnitude and direction with changes in the direction of the applied force and changes in the configuration of the robot.  A parametric plot of the acceleration vector over the direction space of the applied force yields the "ellipsoid of mobility" of the end of the terminal link.

This notebook uses two methods to find the ellipsoid of mobility.  First, the robotic arm is modeled with reference point coordinates giving it a total of 12 degrees of freedom (6 per body).  Constraints are applied to represent the joints of the robot leaving 3 unconstrained degrees of freedom, one per joint angle.  After defining mass properties for each of the links, the ellipsoid of mobility is found by applying a unit force to the end of the terminal link and measuring the resulting acceleration over the direction space of the applied force.  This technique may be applied to spatial models of arbitrary complexity and coupling. 

The second method uses generaized coordinates to create a model with only three degrees of freedom, one for each free joint angle.  From this (much simpler) model, the generalized mass matrix (Subscript[M, G]) is obtained and transformed into an inverse mass matrix (M^(-1)) = J . ((Subscript[M, G])^(-1)) . (J^T) with respect to the end of the terminal link, where J is the Jacobian of end point velocity with respect to the generalized coordinates.  The ellipsoid of mobility can now be obtained ploting the acceleration vector A = (M^(-1)) . F(φ, ϕ).

Setup

Load the MechanicalSystems package.

Needs["Mech`Mech3D`"]Needs["Mech`GenCrd3D`"]$MechVersion

Here are numbers for each body in the model, and convenient unit vector definitions.

base = 1; link1 = 2; link2 = 3; X = {1, 0, 0}; Y = {0, 1, 0}; Z = {0, 0, 1};

Reference Point Coordinates

Constraints

Five constraints are used to define the robot model.  The first two constraints represent the two joints of the robot, and eliminate 9 of the total 12 degrees of freedom.  The OrthoRevolute4 constraint simply models a pair of revolute joints with intersecting orthogonal axes.

\(\(cs[1] = 
      OrthoRevolute4[1, 
		Axis[link1, 0, Y], 
		Axis[base, 0, Z, Y]]; 
    \)\)

\(\(cs[2] = 
      Revolute5[2, 
		Axis[link2, 0, Y], 
		Axis[link1, 12\ Z, Y]]; 
    \)\)

The last three constraints specify the three joint angles to position the robot arm.

cs[3] = ProjectedAngle1[3, Vector[link1, Y], Y, -Z, θ1];

\(\(cs[4] = 
      ProjectedAngle1[4, 
		Vector[link1, Z], 
		Z, 
		
        Vector[link1, Y], θ2]; \)\)

\(\(cs[5] = 
      ProjectedAngle1[5, 
		Vector[link2, Z], 
		Vector[link1, Z], 

        		Vector[link1, Y], θ3]; \)\)

SetConstraints incorporates the constraints into the model.

SetConstraints[cs[1], cs[2], cs[3], cs[4], cs[5]]

Solution

We assign values to the three joint angles and solve the model with SolveMech.

SetParameters[θ1 -> 0.2, θ2 -> 0.5, 
      θ3 -> 0.7]; SolveMech[]

To move the robot to a new configuration we change the joint angles and re-solve.

SetParameters[θ1 -> 0.2, θ2 -> 1.1, 
      θ3 -> 1.5]; SolveMech[];

Here are the global coordinates of the location of the tip of link 2, in terms of the origin coordinates and quaternion of link 2.

\(ptip = {0,  .75, 12}; 
link2tip = Location[link2, ptip]\)

The symbols in link2tip are replaced with values from the last solution found by SolveMech.

link2tip /. LastSolve[]

Graphics

Graphics Objects

The following graphics commands are used to create the robot graphics in this notebook.

\(\(baseGr := 
      Graphics3D[{
			
          Bar[Line[base, {0, 0, \(-3\)}, {0, 0, \(-12\)}], {3, 5}, 15]}]; 
    \)\)

link1Gr := Graphics3D[{Facet[link1,
  a = {{0,-2,-3}, {2.12,-2,-2.12}, {3,-2,0}, {2,-2,12},
    {1.41,-2,13.41}, {0,-2,14}, {-1.41,-2,13.41},
    {-2,-2,12}, {-3,-2,0}, {-2.12,-2,-2.12}, {0,-2,-3}}],
  Extrude[Point[link1, #]&/@a, Vector[link1, Y], 2]}];

\(\(link2Gr := 
      Graphics3D[{
          Facet[link2, 
            a = 
					{{0, 0, \(-2\)}, {1.41, 0, \(-1.41\)}, {2, 0, 
                  0}, {0.5, 0, 12}, {\(-0.5\), 0, 12}, {\(-2\), 0, 0}, {
                  \(-1.41\), 0, \(-1.41\)}, {0, 0, \(-2\)}}], 
			
          Extrude[\((Point[link2, #1]&)\)/@a, Vector[link2, Y], 1.5], 
			
          Sphere[Axis[link2, ptip, Z], 1.3, 10], 
			Thickness[0.01], 

          			Arrow[Axis[Location[link2, ptip] + 6  X, \(-X\)], 4], 
			
          Text[StyleForm["F", FontWeight -> "Bold", 
              FontFamily -> "Times"], 
            Location[link2, ptip] + 5  X + Z]}]; \)\)

robotGraphic := {baseGr,  link1Gr,  link2Gr};

Display

Here is a graphic of the robot arm in its current configuation, as returned by LastSolve[].

\(\(Show[robotGraphic /. LastSolve[], 
	
      PlotLabel -> "Robot Arm"]; \)\)

Graphic Deleted

Mass Properties

SetBodies is used to define mass properties of each body.

\(SetBodies[
	
      Body[link1, Mass -> 3, Inertia -> {100, 120, 10, 0, 0, 0}], 

      	Body[link2, Mass -> 2, Inertia -> {60, 80, 5, 0, 0, 0}]]
      \)

Ellipsoid of Mobility

The ellipsoid of mobility of the tip of link 2 is obtained by applying a unit force to the tip and measuring the resulting acceleration.

First, a velocity solution is obtained only to provide initial conditions for SetFree.  In the absense of initial velocities, SetFree will assume them to be zero which, in this case, is correct.

SolveMech[Solution -> Velocity];

A unit load is applied to the tip of the arm, the direction of which is specified by two direction angles ϕ and θ

\(SetLoads[
      Force[link2, 
        Axis[
			Point[link2, ptip], 
          Vector[base, {Cos[θ] Cos[ϕ], Cos[θ] Sin[ϕ], 
              Sin[θ]}]], 
		1, Magnitude -> Relative]]\)

Here is a FreeSystem object with three degrees of freedom - one for each joint angle.  The first argument of SetFree is a list of constraint numbers to drop from the model to make it underconstrained.

freeSys = SetFree[{3, 4, 5}, Solution -> Dynamic];

Here is a parametric plot of the acceleration vector of the arm as a function of the direction of the applied force.

\(\(ParametricPlot3D[
	
      Acceleration[link2, \ ptip] /. SolveFree[freeSys], 

      	{θ, \(-π\)/2, π/2}, {ϕ, 0, 2  π}, 
	
      PlotPoints -> {10, 16}, 
	
      PlotLabel -> "Ellipsoid of Mobility", 
	
      Compiled -> False]; \)\)

Generalized Coordinates

Constraints

Three generalized coordinates and three constraints are used to define the robot model.  The first two generalized constraint objects represent the two joints of the robot, and add 3 degrees of freedom to the model.  The last three constraints specify the three joint angles to position the robot arm.

\(\(cs[1] = 
      GenOrthoRevolute4[1, 
		Axis[link1, 0, Y], 
		
        Axis[base, 0, Z, Y], 
		{θ1, 0}, {θ2, 0}]; \)\)

\(\(cs[2] = 
      GenRevolute5[2, 
		Axis[link2, 0, Y], 
		Axis[link1, 12\ Z, Y], 

        		{θ3, 0}]; \)\)

\(cs[3] = Constraint[3, θ1 == 0.2]; 

    cs[4] = Constraint[4, θ2 == 1.1]; 

    cs[5] = Constraint[5, θ3 == 1.5]; \)

SetConstraints[cs[1], cs[2], cs[3], cs[4], cs[5]]

The return value of SolveMech, for a model that uses only generalized coordinates, is relatively uninteresting - SolveMech simply returns the specified values of the generalized coordinates.

SolveMech[]

The generalized coordinates actually appear in the model in the results of all of the measurement functions.  For example, Location now returns the location of a local point in terms of generalized coordinates instead of the generic reference point expression that was returned
previously.

link2tip = Location[link2, ptip]

link2tip /. LastSolve[]

Graphics

Here is the same robot graphic, produced with the generalized coordinate model.

\(\(Show[robotGraphic /. LastSolve[], 
	
      PlotLabel -> "Robot Arm"]; \)\)

Graphic Deleted

Generalized Mass Matrix

MassMatrix[All] returns the current system mass matrix, formed with respect to the current coordinate system.  In this case, MassMatrix returns the mass matrix with respect to the three generalized coordinates.

M = MassMatrix[All];

The mass matrix is transformed into endpoint coordinates with the Jacobian of the enpoint of the terminal link ⟶  P = J . (M^(-1)) . (J^T).  Note that we evalute the numerical mass matrix before inverting it - it's symbolic inverse is rather large.

J = Outer[D, link2tip, {θ1, θ2, θ3}];

P = 
      (J /. LastSolve[]) . Inverse[M /. LastSolve[]] . 
        Transpose[J /. LastSolve[]]

Here is another plot of the ellipsoid of mobility A = (M^(-1)) . F(θ, ϕ).

\(\(ParametricPlot3D[
	
      P . {Cos[θ] Cos[ϕ], Cos[θ] Sin[ϕ], 
          Sin[θ]}, 

      	{θ, \(-π\)/2, π/2}, {ϕ, 0, 2  π}, 
	
      PlotPoints -> {10, 16}, 
	
      PlotLabel -> "Ellipsoid of Mobility", 
	
      Compiled -> False]; \)\)