PLaSM

%-------------------------------------  2D arm definition (3 DOFs)
-------------------------------------%

DEF DOF (alpha::IsReal) =

  T:2:-18 ~ (R:<1,2>:(PI/180 * alpha));

DEF arm (alpha1,alpha2,alpha3::IsReal) = STRUCT:

  <rod, DOF:alpha1, rod, DOF:alpha2, rod, DOF:alpha3, rod >

WHERE

  rod = T:<1,2>:<-1,-19>:(CUBOID:<2,20>)

END;

%-------------------------------------  Two arm configurations (degrees) -
--------------------------------------%

arm:<30,45,60>;

arm:<60,45,30>;

%-------------------------------------
Bezier curve
-------------------------------------%

DEF Fact (n::IsInt) =  *:(CAT:<<1>, 2..n>);

DEF BinCoeff (n,i::IsInt) = Fact:n / (Fact:i * Fact:(n-i));

DEF Bernstein (n::IsInt)(i::IsInt) =

  * ~ [K:(BinCoeff:<n,i>),** ~ [ID,K:i],** ~ [- ~ [K:1,ID],K:(n-i)]] ~ S1;

DEF BernsteinBase (n::IsInt) = AA:(Bernstein:n):(0..n);

DEF Bezier (ControlPoints::IsSeq) = (CONS~AA:(+~AA:*~TRANS)~DISTL):

    <BernsteinBase:degree,(AA:(AA:K)~TRANS):ControlPoints>

WHERE

  degree = LEN:ControlPoints - 1

END;

%-------------------------------------
Path in Configuration Space (cubic Bezier)
-------------------------------------%

DEF CSpath = Bezier:<<0,0,0>,<90,0,0>,<90,90,0>,<90,90,90>>;

DEF Intervals (n::IsInt) = QUOTE:(#:n:(1/n));

DEF Path = MAP:CSpath:(Intervals:18);

DEF ScaleFact = SIZE:1:Path;

STRUCT:<     

  S:<1,2,3>:(#:3:ScaleFact):MKframe,   %--Load file DRAWVECT.PSM--%

  Path

>;

%-------------------------------------
Set of configurations in Working
-------------------------------------%

DEF Sampling (n::IsIntPos) = (AA:LIST~AA:/~DISTR):<0..n,n>;

(STRUCT~AA:arm~AA:CSpath):(Sampling:18);