% MotionGenesis file: MGHexapodEI.txt % Problem: Inverse kinematics of Hexapod (Stewart Platform). % Copyright (c) 2009-2023 Motion Genesis LLC. All rights reserved. %----------------------------------- % Given: Position vector from Ao to AHo: px, py, pz % Given: Orientation of AH in A: q1, q2, q3 % Find: Leg lengths: LB, LC, ..., LG %----------------------------------- % Given: Measures of v_AHo_A>: vx, vy vz % Given: Measures of w_AH_A>: wx, wy, wz % Find: Leg length extensions: LB', LC', ..., LG' %-------------------------------------------------------------------- % Bottom platform (A) and top platform (AH). RigidBody A RigidBody AH %-------------------------------------------------------------------- % Points on bottom platform and top platform, respectively. Point B(A), C(A), D(A), E(A), F(A), G(A) Point BH(AH), CH(AH), DH(AH), EH(AH), FH(AH), GH(AH) %-------------------------------------------------------------------- % Given: Physical dimensions for bottom and top platform. Constant Bx = 0.00 m, By = 0.00 m, Bz = 0.00 m Constant Cx = 5.00 m, Cy = 8.66 m, Cz = 0.00 m Constant Dx = 15.0 m, Dy = 8.66 m, Dz = 0.00 m Constant Ex = 20.0 m, Ey = 0.00 m, Ez = 0.00 m Constant Fx = 15.0 m, Fy = -8.66 m, Fz = 0.00 m Constant Gx = 5.00 m, Gy = -8.66 m, Gz = 0.00 m Constant BHx = 0.00 m, BHy = 0.00 m, BHz = 0.00 m Constant CHx = 0.00 m, CHy = 0.00 m, CHz = 0.00 m Constant DHx = 15.0 m, DHy = 8.66 m, DHz = 0.00 m Constant EHx = 15.0 m, EHy = 8.66 m, EHz = 0.00 m Constant FHx = 30.0 m, FHy = 0.00 m, FHz = 0.00 m Constant GHx = 30.0 m, GHy = 0.00 m, GHz = 0.00 m %-------------------------------------------------------------------- % Unknowns: Leg lengths and extension-rates. Variable LB', LC', LD', LE', LF', LG' %-------------------------------------------------------------------- % Knowns: Position and orientation of AH in A. Variable px, py, pz % Ai> measures of AHo's position from Ao. Variable q1, q2, q3 % BodyXYZ Euler angles for AH in A. Variable vx, vy, vz % AHi> measures of AHo's velocity in A. Variable wx, wy, wz % AHi> measures of AH's angular velocity in A. %-------------------------------------------------------------------- % Position vectors from bottom plate construction. B.SetPosition( Ao, Vector( A, Bx, By, Bz ) ) C.SetPosition( Ao, Vector( A, Cx, Cy, Cz ) ) D.SetPosition( Ao, Vector( A, Dx, Dy, Dz ) ) E.SetPosition( Ao, Vector( A, Ex, Ey, Ez ) ) F.SetPosition( Ao, Vector( A, Fx, Fy, Fz ) ) G.SetPosition( Ao, Vector( A, Gx, Gy, Gz ) ) %-------------------------------------------------------------------- % Position vectors from top plate construction. BH.SetPosition( AHo, Vector( AH, BHx, BHy, BHz ) ) CH.SetPosition( AHo, Vector( AH, CHx, CHy, CHz ) ) DH.SetPosition( AHo, Vector( AH, DHx, DHy, DHz ) ) EH.SetPosition( AHo, Vector( AH, EHx, EHy, EHz ) ) FH.SetPosition( AHo, Vector( AH, FHx, FHy, FHz ) ) GH.SetPosition( AHo, Vector( AH, GHx, GHy, GHz ) ) %-------------------------------------------------------------------- % Geometry relating unit vectors. AH.SetRotationMatrix( A, BodyXYZ, q1, q2, q3 ) %-------------------------------------------------------------------- % Given: AHo position from Ao. AHo.SetPosition( Ao, Vector( A, px, py, pz ) ) %-------------------------------------------------------------------- % Calculate the length of each leg (symbolically). LB = BH.GetDistance( B ) LC = CH.GetDistance( C ) LD = DH.GetDistance( D ) LE = EH.GetDistance( E ) LF = FH.GetDistance( F ) LG = GH.GetDistance( G ) %-------------------------------------------------------------------- % Known: Velocity of AHo in A and angular velocity of AH in A. AH.SetAngularVelocity( A, Vector( AH, wx, wy, wz ) ) AHo.SetVelocity( A, Vector( A, vx, vy, vz ) ) %-------------------------------------------------------------------- % Calculate the velocity of each point on top plate. BH.SetVelocity( A, AHo ) CH.SetVelocity( A, AHo ) DH.SetVelocity( A, AHo ) EH.SetVelocity( A, AHo ) FH.SetVelocity( A, AHo ) GH.SetVelocity( A, AHo ) %-------------------------------------------------------------------- % Calculate the extension of each leg (symbolically). LB' = Dot( BH.GetPosition(B)/LB, BH.GetVelocity(A) ) LC' = Dot( CH.GetPosition(C)/LC, CH.GetVelocity(A) ) LD' = Dot( DH.GetPosition(D)/LD, DH.GetVelocity(A) ) LE' = Dot( EH.GetPosition(E)/LE, EH.GetVelocity(A) ) LF' = Dot( FH.GetPosition(F)/LF, FH.GetVelocity(A) ) LG' = Dot( GH.GetPosition(G)/LG, GH.GetVelocity(A) ) %-------------------------------------------------------------------- % Input known values for numerical solution. Input px = 1 m, py = 2 m, pz = 20 m Input q1 = 25 deg, q2 = 40 deg, q3 = 50 deg Input vx = 0.4 m/s, vy = 0.5 m/s, vz = 0.6 m/s Input wx = 0.1 rad/s, wy = 0.2 rad/s, wz = 0.3 rad/s %-------------------------------------------------------------------- % Calculate leg lengths for given numerical values (numerics). LBo = EvaluateToNumber( LB ) % Should be LBo = 20.12461 LCo = EvaluateToNumber( LC ) % Should be LCo = 21.4559 LDo = EvaluateToNumber( LD ) % Should be LDo = 29.61912 LEo = EvaluateToNumber( LE ) % Should be LEo = 35.50773 LFo = EvaluateToNumber( LF ) % Should be LFo = 41.12047 LGo = EvaluateToNumber( LG ) % Should be LGo = 42.501 %-------------------------------------------------------------------- % Calculate leg extension rates for given numerical values (numerics). LB_ = EvaluateToNumber( LB' ) % Should be LB' = 0.6658514 LC_ = EvaluateToNumber( LC' ) % Should be LC' = 0.3295131 LD_ = EvaluateToNumber( LD' ) % Should be LD' = 4.264621 LE_ = EvaluateToNumber( LE' ) % Should be LE' = 4.397064 LF_ = EvaluateToNumber( LF' ) % Should be LF' = 6.328711 LG_ = EvaluateToNumber( LG' ) % Should be LG' = 4.067154 %-------------------------------------------------------------------- Save MGHexapodEI.html Quit Input px= 6.77410E-01 m, py= 1.26938E-01 m, pz= 2.50000E+01 m