├── .gitignore ├── 3D ├── rq.m ├── rqd.m ├── rv.m ├── rx.m ├── ry.m ├── rz.m └── skew.m ├── README.md ├── READ_ME.txt ├── dynamics ├── FDcrb.m ├── FDfb.m ├── FDgq.m ├── HD.m ├── IDfb.m ├── apply_external_forces.m ├── fbanim.m ├── fbkin.m └── get_gravity.m ├── models ├── autoTree.m ├── doubleElbow.m ├── floatbase.m ├── gfxExample.m ├── planar.m ├── rollingdisc.m ├── scissor.m ├── singlebody.m └── vee.m ├── showmotion.m ├── simulink ├── ex4_anal.m ├── example1.mdl ├── example2.mdl ├── example3.mdl ├── example4.mdl ├── example5.mdl ├── example6.mdl ├── example7.mdl ├── gcFD.m ├── gcPosVel.m └── gcontact.m ├── sparsity ├── LTDL.m ├── LTL.m ├── expandLambda.m ├── mpyH.m ├── mpyL.m ├── mpyLi.m ├── mpyLit.m └── mpyLt.m ├── spatial ├── Fpt.m ├── Vpt.m ├── Xpt.m ├── XtoV.m ├── crf.m ├── crm.m ├── mcI.m ├── plnr.m ├── pluho.m ├── plux.m ├── rotx.m ├── roty.m ├── rotz.m └── xlt.m ├── startup.m └── v3 ├── 3D ├── inv_skew.m ├── se3toVec.m └── vecTose3.m ├── derivatives ├── FD_derivatives.m ├── H_derivatives.m ├── ID_SO_derivatives.m ├── ID_derivatives.m ├── StructureConstants.m ├── modFD_derivatives.m ├── modFD_second_derivatives.m ├── modID_derivatives.m ├── modID_derivatives_simple.m └── modID_second_derivatives.m ├── dynamics ├── BodyJacobian.m ├── BodyJacobianTimeDerivative.m ├── CMM.m ├── CMMTimeDerivative.m ├── CMM_from_CRBA.m ├── Christoffel.m ├── CoriolisMatrix.m ├── EnerMo.m ├── FDab.m ├── HamiltonianDynamics.m ├── HandC.m ├── Hinverse.m ├── Hqd_and_CTqd.m ├── ID.m ├── ID_SlotineLi.m ├── fbkin.m ├── jcalc.m ├── jinfo.m ├── modFD.m └── modID.m ├── identifiability ├── Algorithms │ ├── ComputeBases.m │ ├── ComputeBases_rotor.m │ ├── RPNA.m │ └── RPNA_rotor.m ├── RPNA_Examples.m ├── RPNA_URDF_Example.m └── SupportFunctions │ ├── AutoGenerated │ ├── CreateAutoGeneratedFunctions.m │ ├── Output_InnerProduct.m │ ├── Output_InnerProduct_rotor.m │ ├── Rate_Parameters.m │ └── Transform_Parameters.m │ ├── CleanMat.m │ ├── ComputeSampledRegressor.m │ ├── IsIdentifiable.m │ ├── IsIdentifiable_rotor.m │ ├── IsUnidentifiable.m │ ├── IsUnidentifiable_rotor.m │ ├── OutputMatrix.m │ ├── OutputMatrix_rotor.m │ ├── PrintParameterSummary.m │ ├── PrintParameterSummary_rotor.m │ ├── RangeBasis.m │ ├── SwitchedControllabilityMatrix.m │ ├── SwitchedObservabilityMatrix.m │ ├── UnitVectorComplementarySubspace.m │ └── unitVector.m ├── inertia ├── entropicDivergence.m ├── getModelInertialParams.m ├── inertiaMatToPinertia.m ├── inertiaMatToVec.m ├── inertiaVecToMat.m ├── inertiaVecToPinertia.m ├── pinertiaToVec.m └── pullbackMetric.m ├── models ├── CheetahLeg_model.m ├── Panda_model.m ├── Puma560_model.m ├── Scara_model.m ├── TX40_model.m ├── Test_model.m ├── URDF_to_spatialv2_model.m ├── autoTree_rotor.m ├── panda_arm_no_fixed.urdf ├── panda_basis_from_DH.mat ├── panda_basis_from_URDF.mat └── puma560_robot.urdf ├── orientation_tools ├── AngleAxis │ ├── angleAxisRateLeft.m │ ├── angleAxisRateRight.m │ ├── angleAxisToAngleAndAxis.m │ ├── angleAxisToCayley.m │ ├── angleAxisToMRP.m │ ├── angleAxisToQuat.m │ ├── angleAxisToRot.m │ └── angleAxisToRpy.m ├── Cayley │ ├── cayleyProduct.m │ ├── cayleyRateLeft.m │ ├── cayleyRateRight.m │ ├── cayleyToAngleAxis.m │ ├── cayleyToMRP.m │ ├── cayleyToQuat.m │ ├── cayleyToRot.m │ └── cayleyToRpy.m ├── MRP │ ├── mrpProduct.m │ ├── mrpRateLeft.m │ ├── mrpRateRight.m │ ├── mrpShadow.m │ ├── mrpToAngleAxis.m │ ├── mrpToCayley.m │ ├── mrpToQuat.m │ ├── mrpToRot.m │ └── mrpToRpy.m ├── Quaternion │ ├── SU2toQuat.m │ ├── quatConj.m │ ├── quatL.m │ ├── quatProduct.m │ ├── quatR.m │ ├── quatRateLeft.m │ ├── quatRateRight.m │ ├── quatToAngleAxis.m │ ├── quatToCayley.m │ ├── quatToMRP.m │ ├── quatToRot.m │ ├── quatToRpy.m │ └── quatToSU2.m ├── RPY │ ├── rpyToAngleAxis.m │ ├── rpyToCayley.m │ ├── rpyToMRP.m │ ├── rpyToQuat.m │ └── rpyToRot.m ├── Rot │ ├── rotToAngleAxis.m │ ├── rotToCayley.m │ ├── rotToEulerAngles.m │ ├── rotToFixedAngles.m │ ├── rotToMRP.m │ ├── rotToQuat.m │ └── rotToRpy.m └── Util │ ├── Rx.m │ ├── Ry.m │ ├── Rz.m │ ├── isEqual.m │ ├── isEqualModSign.m │ └── skew.m ├── regressor ├── RegressorClassical.m ├── RegressorHandG.m ├── RegressorSL.m ├── Regressor_HqdandCTqd.m └── individualRegressor.m ├── spatial ├── AdjointToSE3.m ├── SE3toAdjoint.m ├── crmExtract.m ├── factorFunctions.m └── icrf.m ├── unit_tests ├── UnitTest_All.m ├── UnitTest_CMM.m ├── UnitTest_Coriolis.m ├── UnitTest_Derivatives.m ├── UnitTest_Derivatives_FB.m ├── UnitTest_Dynamics.m ├── UnitTest_Identifiability_Basis.m ├── UnitTest_Identifiability_Basis_via_load.m ├── UnitTest_Orientation.m ├── UnitTest_OrientationRates.m └── UnitTest_URDF_import.m └── utility ├── affineInvariantDistance.m ├── configuration ├── confVecToCell.m ├── configurationAddition.m ├── configurationRates.m └── normalizeConfVec.m ├── finite_diff ├── complexStepJacobian.m ├── finiteDiff.m └── finiteDiffJacobian.m ├── lie_group_tools └── rightJacobianOfExp.m ├── logdetBregmanDivergence.m ├── outputSelect.m ├── postProcessModel.m ├── random ├── randomAdSE3.m ├── randomInertia.m ├── randomInertialParams.m ├── randomPinertia.m ├── randomPositiveDefinite.m ├── randomQuat.m ├── randomRotation.m ├── randomSE3.m ├── randomSkew.m ├── randomSymmetric.m └── random_se3.m ├── symbolic ├── symbolicCartesian.m ├── symbolicForce.m ├── symbolicInertia.m ├── symbolicInertialParams.m └── symbolicVelocity.m └── tensor ├── tensorRotR.m └── tensorRotT.m /.gitignore: -------------------------------------------------------------------------------- 1 | documentation 2 | *.m~ 3 | v3/original 4 | v3/derivatives/old 5 | v3/sandbox 6 | v3/testing 7 | -------------------------------------------------------------------------------- /3D/rq.m: -------------------------------------------------------------------------------- 1 | function out = rq( in ) 2 | 3 | % rq unit quaternion <--> 3x3 coordinate rotation matrix 4 | % E=rq(q) and q=rq(E) convert between a unit quaternion q, representing 5 | % the orientation of a coordinate frame B relative to frame A, and the 3x3 6 | % coordinate rotation matrix E that transforms from A to B coordinates. 7 | % For example, if B is rotated relative to A about their common X axis by 8 | % an angle h, then q=[cos(h/2);sin(h/2);0;0] and rq(q) produces the same 9 | % matrix as rx(h). If the argument is a 3x3 matrix then it is assumed to 10 | % be E, otherwise it is assumed to be q. rq(E) expects E to be accurately 11 | % orthonormal, and returns a quaternion in a 4x1 matrix; but rq(q) accepts 12 | % any nonzero quaternion, contained in either a row or a column vector, and 13 | % normalizes it before use. As both q and -q represent the same rotation, 14 | % rq(E) returns the value that satisfies q(1)>0. If q(1)==0 then it picks 15 | % the value such that the largest-magnitude element is positive. In the 16 | % event of a tie, the smaller index wins. 17 | 18 | if all(size(in)==[3 3]) 19 | out = Etoq( in ); 20 | else 21 | out = qtoE( in ); 22 | end 23 | 24 | 25 | function E = qtoE( q ) 26 | 27 | q = q / norm(q); 28 | 29 | q0s = q(1)*q(1); 30 | q1s = q(2)*q(2); 31 | q2s = q(3)*q(3); 32 | q3s = q(4)*q(4); 33 | q01 = q(1)*q(2); 34 | q02 = q(1)*q(3); 35 | q03 = q(1)*q(4); 36 | q12 = q(2)*q(3); 37 | q13 = q(4)*q(2); 38 | q23 = q(3)*q(4); 39 | 40 | E = 2 * [ q0s+q1s-0.5, q12 + q03, q13 - q02; 41 | q12 - q03, q0s+q2s-0.5, q23 + q01; 42 | q13 + q02, q23 - q01, q0s+q3s-0.5 ]; 43 | 44 | 45 | function q = Etoq( E ) 46 | 47 | % for sufficiently large q0, this function formulates 2*q0 times the 48 | % correct return value; otherwise, it formulates 4*|q1| or 4*|q2| or 4*|q3| 49 | % times the correct value. The final normalization step yields the correct 50 | % return value. 51 | 52 | tr = trace(E); % trace is 4*q0^2-1 53 | v = -skew(E); % v is 2*q0 * [q1;q2;q3] 54 | 55 | if tr > 0 56 | q = [ (tr+1)/2; v ]; 57 | else 58 | E = E - (tr-1)/2 * eye(3); 59 | E = E + E'; 60 | if E(1,1) >= E(2,2) && E(1,1) >= E(3,3) 61 | q = [ 2*v(1); E(:,1) ]; 62 | elseif E(2,2) >= E(3,3) 63 | q = [ 2*v(2); E(:,2) ]; 64 | else 65 | q = [ 2*v(3); E(:,3) ]; 66 | end 67 | if q(1) < 0 68 | q = -q; 69 | end 70 | end 71 | 72 | q = q / sqrt( q.'*q); 73 | -------------------------------------------------------------------------------- /3D/rqd.m: -------------------------------------------------------------------------------- 1 | function qd = rqd( in1, in2 ) 2 | 3 | % rqd derivative of unit quaternion from angular velocity 4 | % qd=rqd(wA,q) and qd=rqd(q,wB) calculate the derivative of a unit 5 | % quaternion, q, representing the orientation of a coordinate frame B 6 | % relative to frame A, given the angular velocity w of B relative to A. If 7 | % w is expressed in A coordinates then use rqd(wA,q); and if w is expressed 8 | % in B coordinates then use rqd(q,wB). If the length of the first argument 9 | % is 4 then it is assumed to be q, otherwise it is assumed to be wA. The 10 | % return value is a column vector, but the arguments can be row or column 11 | % vectors. It is not necessary for |q| to be exactly 1. If |q|~=1 then qd 12 | % contains a magnitude-stabilizing term that will cause |q| to converge 13 | % towards 1 if q is obtained by numerical integration of qd. 14 | 15 | Kstab = 0.1; % magnitude stabilization constant: 16 | % value not critical, but K>1 too big 17 | 18 | if length(in1) == 4 % arguments are q and wB 19 | q = in1; 20 | w(1:3,1) = in2; 21 | Q = [ q(1) -q(2) -q(3) -q(4); 22 | q(2) q(1) -q(4) q(3); 23 | q(3) q(4) q(1) -q(2); 24 | q(4) -q(3) q(2) q(1) ]; 25 | else % arguments are wA and q 26 | q = in2; 27 | w(1:3,1) = in1; 28 | Q = [ q(1) -q(2) -q(3) -q(4); 29 | q(2) q(1) q(4) -q(3); 30 | q(3) -q(4) q(1) q(2); 31 | q(4) q(3) -q(2) q(1) ]; 32 | end 33 | 34 | qd = 0.5 * Q * [ Kstab*norm(w)*(1-norm(q)); w ]; 35 | -------------------------------------------------------------------------------- /3D/rv.m: -------------------------------------------------------------------------------- 1 | function out = rv( in ) 2 | 3 | % rv 3D rotation vector <--> 3x3 coordinate rotation matrix 4 | % E=rv(v) and v=rv(E) convert between a rotation vector v, whose magnitude 5 | % and direction describe the angle and axis of rotation of a coordinate 6 | % frame B relative to frame A, and the 3x3 coordinate rotation matrix E 7 | % that transforms from A to B coordinates. For example, if v=[theta;0;0] 8 | % then rv(v) produces the same matrix as rx(theta). If the argument is a 9 | % 3x3 matrix then it is assumed to be E, otherwise it is assumed to be v. 10 | % rv(E) expects E to be accurately orthonormal, and returns a column vector 11 | % with a magnitude in the range [0,pi]. If the magnitude is exactly pi 12 | % then the sign of the return value is unpredictable, since pi*u and -pi*u, 13 | % where u is any unit vector, both represent the same rotation. rv(v) will 14 | % accept a row or column vector of any magnitude. 15 | 16 | if all(size(in)==[3 3]) 17 | out = Etov( in ); 18 | else 19 | out = vtoE( [in(1);in(2);in(3)] ); 20 | end 21 | 22 | 23 | function E = vtoE( v ) 24 | 25 | theta = norm(v); 26 | if theta == 0 27 | E = eye(3); 28 | else 29 | s = sin(theta); 30 | c = cos(theta); 31 | c1 = 2 * sin(theta/2)^2; % 1-cos(h) == 2sin^2(h/2) 32 | u = v/theta; 33 | E = c*eye(3) - s*skew(u) + c1*u*u'; 34 | end 35 | 36 | 37 | function v = Etov( E ) 38 | 39 | % This function begins by extracting the skew-symmetric component of E, 40 | % which, as can be seen from the previous function, is -s*skew(v/theta). 41 | % For angles sufficiently far from pi, v is then calculated directly from 42 | % this quantity. However, for angles close to pi, E is almost symmetric, 43 | % and so extracting the skew-symmetric component becomes numerically 44 | % ill-conditioned, and provides an increasingly inaccurate value for the 45 | % direction of v. Therefore, the component c1*u*u' is extracted as well, 46 | % and used to get an accurate value for the direction of v. If the angle 47 | % is exactly pi then the sign of v will be indeterminate, since +v and -v 48 | % both represent the same rotation, but the direction will still be 49 | % accurate. 50 | 51 | w = -skew(E); % w == s/theta * v 52 | s = norm(w); 53 | c = (trace(E)-1)/2; 54 | theta = atan2(s,c); 55 | 56 | if s == 0 57 | v = [0;0;0]; 58 | elseif theta < 0.9*pi % a somewhat arbitrary threshold 59 | v = theta/s * w; 60 | else % extract v*v' component from E and 61 | E = E - c * eye(3); % pick biggest column (best chance 62 | E = E + E'; % to get sign right) 63 | if E(1,1) >= E(2,2) && E(1,1) >= E(3,3) 64 | if w(1) >= 0 65 | v = E(:,1); 66 | else 67 | v = -E(:,1); 68 | end 69 | elseif E(2,2) >= E(3,3) 70 | if w(2) >= 0 71 | v = E(:,2); 72 | else 73 | v = -E(:,2); 74 | end 75 | else 76 | if w(3) >= 0 77 | v = E(:,3); 78 | else 79 | v = -E(:,3); 80 | end 81 | end 82 | v = theta/norm(v) * v; 83 | end 84 | -------------------------------------------------------------------------------- /3D/rx.m: -------------------------------------------------------------------------------- 1 | function E = rx( theta ) 2 | 3 | % rx 3x3 coordinate rotation (X-axis) 4 | % rx(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common X axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | E = [ 1 0 0; 12 | 0 c s; 13 | 0 -s c ]; 14 | -------------------------------------------------------------------------------- /3D/ry.m: -------------------------------------------------------------------------------- 1 | function E = ry( theta ) 2 | 3 | % ry 3x3 coordinate rotation (Y-axis) 4 | % ry(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common Y axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | E = [ c 0 -s; 12 | 0 1 0; 13 | s 0 c ]; 14 | -------------------------------------------------------------------------------- /3D/rz.m: -------------------------------------------------------------------------------- 1 | function E = rz( theta ) 2 | 3 | % rz 3x3 coordinate rotation (Z-axis) 4 | % rz(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common Z axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | E = [ c s 0; 12 | -s c 0; 13 | 0 0 1 ]; 14 | -------------------------------------------------------------------------------- /3D/skew.m: -------------------------------------------------------------------------------- 1 | function out = skew( in ) 2 | 3 | % skew convert 3D vector <--> 3x3 skew-symmetric matrix 4 | % S=skew(v) and v=skew(A) calculate the 3x3 skew-symmetric matrix S 5 | % corresponding to the given 3D vector v, and the 3D vector corresponding 6 | % to the skew-symmetric component of the given arbitrary 3x3 matrix A. For 7 | % vectors a and b, skew(a)*b is the cross product of a and b. If the 8 | % argument is a 3x3 matrix then it is assumed to be A, otherwise it is 9 | % assumed to be v. skew(A) produces a column-vector result, but skew(v) 10 | % will accept a row or column vector argument. 11 | 12 | if all(size(in)==[3 3]) % do v = skew(A) 13 | out = 0.5 * [ in(3,2) - in(2,3); 14 | in(1,3) - in(3,1); 15 | in(2,1) - in(1,2) ]; 16 | else % do S = skew(v) 17 | out = [ 0, -in(3), in(2); 18 | in(3), 0, -in(1); 19 | -in(2), in(1), 0 ]; 20 | end 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # spatial_v2_extended 2 | 3 | This package directly builds upon Roy Featherstone's [spatial_v2 library](http://royfeatherstone.org/spatial/v2/) and his closely associated [book](https://link.springer.com/book/10.1007/978-1-4899-7560-7). 4 | 5 | New algorithms include: 6 | * Methods to compute the Coriolis matrix ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/blob/main/v3/dynamics/CoriolisMatrix.m)) [all systems] and Christoffel symbols ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/blob/main/v3/dynamics/Christoffel.m)) ([paper](http://dx.doi.org/10.1115/1.4051169)) 7 | * Methods for assessing identifiability ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/tree/main/v3/identifiability), [paper](https://arxiv.org/abs/1711.03896)) 8 | * Methods to calculate second-order partial derivatives of Inverse Dynamics for multi-DoF joints ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/blob/main/v3/derivatives/ID_SO_derivatives.m)) 9 | 10 | New features include: 11 | * Extensions of most algorithms (RNEA, ABA, CRBA, etc.) to address dynamic effects from motor rotors ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/tree/main/v3/dynamics)) 12 | * Updated most algorithms (RNEA, ABA, CRBA, etc.) to handle multi-DoF joints (e.g., spherical or floating base) without needing specialized versions of the algorithms ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/tree/main/v3/dynamics)) 13 | * Regressor calculation algorithms ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/tree/main/v3/regressor)) 14 | * Variety of tools for converting between different representations of orientation ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/tree/main/v3/orientation_tools)) 15 | * Methods for computing the partial derivatives of Inverse Dynamics ([link](https://github.com/ROAM-Lab-ND/spatial_v2_extended/blob/main/v3/derivatives/ID_derivatives.m)) 16 | * Partial compatibility for complex-valued input arguments toward support of complex-step derivative comptuations in unit tests (including the complex step on matrix Lie groups [link](https://ieeexplore-ieee-org.proxy.library.nd.edu/abstract/document/8957301)). 17 | -------------------------------------------------------------------------------- /dynamics/FDcrb.m: -------------------------------------------------------------------------------- 1 | function qdd = FDcrb( model, q, qd, tau, f_ext ) 2 | 3 | % FDcrb Forward Dynamics via Composite-Rigid-Body Algorithm 4 | % FDcrb(model,q,qd,tau,f_ext) calculates the forward dynamics of a 5 | % kinematic tree via the composite-rigid-body algorithm. q, qd and tau are 6 | % vectors of joint position, velocity and force variables; and the return 7 | % value is a vector of joint acceleration variables. f_ext is an optional 8 | % argument specifying the external forces acting on the bodies. It can be 9 | % omitted if there are no external forces. The format of f_ext is 10 | % explained in the source code of apply_external_forces. 11 | 12 | if nargin == 4 13 | [H,C] = HandC( model, q, qd ); 14 | else 15 | [H,C] = HandC( model, q, qd, f_ext ); 16 | end 17 | 18 | qdd = H \ (tau - C); 19 | -------------------------------------------------------------------------------- /dynamics/FDfb.m: -------------------------------------------------------------------------------- 1 | function [xdfb,qdd] = FDfb( model, xfb, q, qd, tau, f_ext ) 2 | 3 | % FDfb Floating-Base Forward Dynamics via Articulated-Body Algorithm 4 | % [xdfb,qdd]=FDfb(model,xfb,q,qd,tau,f_ext) calculates the forward dynamics 5 | % of a floating-base kinematic tree via the articulated-body algorithm. 6 | % This function avoids the kinematic singularity in the six-joint chain 7 | % created by floatbase to mimic a true 6-DoF joint. xfb is a 13-element 8 | % column vector containing: a unit quaternion specifying the orientation of 9 | % the floating base (=body 6)'s coordinate frame relative to the fixed 10 | % base; a 3D vector specifying the position of the origin of the floating 11 | % base's coordinate frame in fixed-base coordinates; and a spatial vector 12 | % giving the velocity of the floating base in fixed-base coordinates. The 13 | % return value xdfb is the time-derivative of this vector. The arguments 14 | % q, qd and tau contain the position, velocity and force variables for the 15 | % real joints in the system (i.e., joints 7 onwards in the system model); 16 | % so q(i), qd(i) and tau(i) all apply to joint i+6. The return value qdd 17 | % is the time-derivative of qd. f_ext is an optional argument specifying 18 | % the external forces acting on the bodies. It can be omitted if there are 19 | % no external forces. If supplied, it must be a cell array of length 20 | % model.NB, of which the first 5 elements are ignored, and f_ext{6} onward 21 | % specify the forces acting on the floating base (body 6) onward. The 22 | % format of f_ext is explained in the source code of apply_external_forces. 23 | 24 | a_grav = get_gravity(model); 25 | 26 | qn = xfb(1:4); % unit quaternion fixed-->f.b. 27 | r = xfb(5:7); % position of f.b. origin 28 | Xup{6} = plux( rq(qn), r ); % xform fixed --> f.b. coords 29 | 30 | vfb = xfb(8:end); 31 | v{6} = Xup{6} * vfb; % f.b. vel in f.b. coords 32 | 33 | IA{6} = model.I{6}; 34 | pA{6} = crf(v{6}) * model.I{6} * v{6}; 35 | 36 | for i = 7:model.NB 37 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i-6) ); 38 | vJ = S{i}*qd(i-6); 39 | Xup{i} = XJ * model.Xtree{i}; 40 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 41 | c{i} = crm(v{i}) * vJ; 42 | IA{i} = model.I{i}; 43 | pA{i} = crf(v{i}) * model.I{i} * v{i}; 44 | end 45 | 46 | if nargin == 6 && length(f_ext) > 0 47 | prnt = model.parent(6:end) - 5; 48 | pA(6:end) = apply_external_forces(prnt, Xup(6:end), pA(6:end), f_ext(6:end)); 49 | end 50 | 51 | for i = model.NB:-1:7 52 | U{i} = IA{i} * S{i}; 53 | d{i} = S{i}' * U{i}; 54 | u{i} = tau(i-6) - S{i}'*pA{i}; 55 | Ia = IA{i} - U{i}/d{i}*U{i}'; 56 | pa = pA{i} + Ia*c{i} + U{i} * u{i}/d{i}; 57 | IA{model.parent(i)} = IA{model.parent(i)} + Xup{i}' * Ia * Xup{i}; 58 | pA{model.parent(i)} = pA{model.parent(i)} + Xup{i}' * pa; 59 | end 60 | 61 | a{6} = - IA{6} \ pA{6}; % floating base accn without gravity 62 | 63 | qdd = zeros(0,1); % avoids a matlab warning when NB==6 64 | 65 | for i = 7:model.NB 66 | a{i} = Xup{i} * a{model.parent(i)} + c{i}; 67 | qdd(i-6,1) = (u{i} - U{i}'*a{i})/d{i}; 68 | a{i} = a{i} + S{i}*qdd(i-6); 69 | end 70 | 71 | qnd = rqd( vfb(1:3), qn ); % derivative of qn 72 | rd = Vpt( vfb, r ); % lin vel of flt base origin 73 | afb = Xup{6} \ a{6} + a_grav; % true f.b. accn in fixed-base coords 74 | 75 | xdfb = [ qnd; rd; afb ]; 76 | -------------------------------------------------------------------------------- /dynamics/HD.m: -------------------------------------------------------------------------------- 1 | function [qdd_out,tau_out] = HD( model, fd, q, qd, qdd, tau, f_ext ) 2 | 3 | % HD Articulated-Body Hybrid Dynamics Algorithm 4 | % [qdd_out,tau_out]=HD(model,fd,q,qd,qdd,tau,f_ext) calculates the hybrid 5 | % dynamics of a kinematic tree using the articulated-body algorithm. fd is 6 | % an array of boolean values such that fd(i)==1 if joint i is a 7 | % forward-dynamics joint, and fd(i)==0 otherwise. If fd(i)==1 then tau(i) 8 | % contains the given force at joint i, and the value of qdd(i) is ignored; 9 | % and if fd(i)==0 then qdd(i) contains the given acceleration at joint i, 10 | % and the value of tau(i) is ignored. Likewise, if fd(i)==1 then 11 | % qdd_out(i) contains the calculated acceleration at joint i, and 12 | % tau_out(i) contains the given force copied from tau(i); and if fd(i)==0 13 | % then tau_out(i) contains the calculated force and qdd_out(i) the given 14 | % acceleration copied from qdd(i). Thus, the two output vectors are always 15 | % fully instantiated. f_ext is an optional argument specifying the 16 | % external forces acting on the bodies. It can be omitted if there are no 17 | % external forces. The format of f_ext is explained in the source code of 18 | % apply_external_forces. 19 | 20 | a_grav = get_gravity(model); 21 | 22 | for i = 1:model.NB 23 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i) ); 24 | vJ = S{i}*qd(i); 25 | Xup{i} = XJ * model.Xtree{i}; 26 | if model.parent(i) == 0 27 | v{i} = vJ; 28 | c{i} = zeros(size(a_grav)); % spatial or planar zero vector 29 | else 30 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 31 | c{i} = crm(v{i}) * vJ; 32 | end 33 | if fd(i) == 0 34 | c{i} = c{i} + S{i} * qdd(i); 35 | end 36 | IA{i} = model.I{i}; 37 | pA{i} = crf(v{i}) * model.I{i} * v{i}; 38 | end 39 | 40 | if nargin == 7 41 | pA = apply_external_forces( model.parent, Xup, pA, f_ext ); 42 | end 43 | 44 | for i = model.NB:-1:1 45 | if fd(i) == 0 46 | if model.parent(i) ~= 0 47 | Ia = IA{i}; 48 | pa = pA{i} + IA{i}*c{i}; 49 | IA{model.parent(i)} = IA{model.parent(i)} + Xup{i}' * Ia * Xup{i}; 50 | pA{model.parent(i)} = pA{model.parent(i)} + Xup{i}' * pa; 51 | end 52 | else 53 | U{i} = IA{i} * S{i}; 54 | d{i} = S{i}' * U{i}; 55 | u{i} = tau(i) - S{i}'*pA{i}; 56 | if model.parent(i) ~= 0 57 | Ia = IA{i} - U{i}/d{i}*U{i}'; 58 | pa = pA{i} + Ia*c{i} + U{i} * u{i}/d{i}; 59 | IA{model.parent(i)} = IA{model.parent(i)} + Xup{i}' * Ia * Xup{i}; 60 | pA{model.parent(i)} = pA{model.parent(i)} + Xup{i}' * pa; 61 | end 62 | end 63 | end 64 | 65 | for i = 1:model.NB 66 | if model.parent(i) == 0 67 | a{i} = Xup{i} * -a_grav + c{i}; 68 | else 69 | a{i} = Xup{i} * a{model.parent(i)} + c{i}; 70 | end 71 | if fd(i) == 0 72 | qdd_out(i,1) = qdd(i); 73 | tau_out(i,1) = S{i}'*(IA{i}*a{i} + pA{i}); 74 | else 75 | qdd_out(i,1) = (u{i} - U{i}'*a{i})/d{i}; 76 | tau_out(i,1) = tau(i); 77 | a{i} = a{i} + S{i}*qdd_out(i); 78 | end 79 | end 80 | -------------------------------------------------------------------------------- /dynamics/IDfb.m: -------------------------------------------------------------------------------- 1 | function [xdfb,tau] = IDfb( model, xfb, q, qd, qdd, f_ext ) 2 | 3 | % IDfb Floating-Base Inverse Dynamics (=Hybrid Dynamics) 4 | % [xdfb,tau]=IDfb(model,xfb,q,qd,qdd,f_ext) calculates the inverse dynamics 5 | % of a floating-base kinematic tree via the algorithm in Table 9.6 of RBDA 6 | % (which is really a special case of hybrid dynamics), using the same 7 | % singularity-free representation of the motion of the floating base as 8 | % used by FDfb. xfb is a 13-element column vector containing: a unit 9 | % quaternion specifying the orientation of the floating base (=body 6)'s 10 | % coordinate frame relative to the fixed base; a 3D vector specifying the 11 | % position of the origin of the floating base's coordinate frame in 12 | % fixed-base coordinates; and a spatial vector giving the velocity of the 13 | % floating base in fixed-base coordinates. The return value xdfb is the 14 | % time-derivative of xfb. The arguments q, qd and qdd contain the 15 | % position, velocity and acceleration variables for the real joints in the 16 | % system (i.e., joints 7 onwards in the system model); so q(i), qd(i) and 17 | % qdd(i) all apply to joint i+6. The return value tau is the vector of 18 | % force variables required to produce the given acceleration qdd. f_ext is 19 | % an optional argument specifying the external forces acting on the bodies. 20 | % It can be omitted if there are no external forces. If supplied, it must 21 | % be a cell array of length model.NB, of which the first 5 elements are 22 | % ignored, and f_ext{6} onward specify the forces acting on the floating 23 | % base (body 6) onward. The format of f_ext is explained in the source 24 | % code of apply_external_forces. 25 | 26 | a_grav = get_gravity(model); 27 | 28 | qn = xfb(1:4); % unit quaternion fixed-->f.b. 29 | r = xfb(5:7); % position of f.b. origin 30 | Xup{6} = plux( rq(qn), r ); % xform fixed --> f.b. coords 31 | 32 | vfb = xfb(8:end); 33 | v{6} = Xup{6} * vfb; % f.b. vel in f.b. coords 34 | 35 | a{6} = zeros(6,1); 36 | 37 | IC{6} = model.I{6}; 38 | pC{6} = model.I{6}*a{6} + crf(v{6})*model.I{6}*v{6}; 39 | 40 | for i = 7:model.NB 41 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i-6) ); 42 | vJ = S{i}*qd(i-6); 43 | Xup{i} = XJ * model.Xtree{i}; 44 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 45 | a{i} = Xup{i}*a{model.parent(i)} + S{i}*qdd(i-6) + crm(v{i})*vJ; 46 | IC{i} = model.I{i}; 47 | pC{i} = IC{i}*a{i} + crf(v{i})*IC{i}*v{i}; 48 | end 49 | 50 | if nargin == 6 && length(f_ext) > 0 51 | prnt = model.parent(6:end) - 5; 52 | pC(6:end) = apply_external_forces(prnt, Xup(6:end), pC(6:end), f_ext(6:end)); 53 | end 54 | 55 | for i = model.NB:-1:7 56 | IC{model.parent(i)} = IC{model.parent(i)} + Xup{i}'*IC{i}*Xup{i}; 57 | pC{model.parent(i)} = pC{model.parent(i)} + Xup{i}'*pC{i}; 58 | end 59 | 60 | a{6} = - IC{6} \ pC{6}; % floating-base acceleration 61 | % without gravity 62 | for i = 7:model.NB 63 | a{i} = Xup{i} * a{model.parent(i)}; 64 | tau(i-6,1) = S{i}'*(IC{i}*a{i} + pC{i}); 65 | end 66 | 67 | qnd = rqd( vfb(1:3), qn ); % derivative of qn 68 | rd = Vpt( vfb, r ); % lin vel of flt base origin 69 | afb = Xup{6} \ a{6} + a_grav; % f.b. accn in fixed-base coords 70 | 71 | xdfb = [ qnd; rd; afb ]; 72 | -------------------------------------------------------------------------------- /dynamics/apply_external_forces.m: -------------------------------------------------------------------------------- 1 | function f_out = apply_external_forces( parent, Xup, f_in, f_ext ) 2 | 3 | % apply_external_forces subtract f_ext from a given cell array of forces 4 | % f_out=apply_external_forces(parent,Xup,f_in,f_ext) incorporates the 5 | % external forces specified in f_ext into the calculations of a dynamics 6 | % algorithm. It does this by subtracting the contents of f_ext from an 7 | % array of forces supplied by the calling function (f_in) and returning the 8 | % result. f_ext has the following format: (1) it must either be an empty 9 | % cell array, indicating that there are no external forces, or else a cell 10 | % array containing NB elements such that f_ext{i} is the external force 11 | % acting on body i; (2) f_ext{i} must either be an empty array, indicating 12 | % that there is no external force acting on body i, or else a spatial or 13 | % planar vector (as appropriate) giving the external force expressed in 14 | % absolute coordinates. apply_external_forces performs the calculation 15 | % f_out = f_in - transformed f_ext, where f_out and f_in are cell arrays of 16 | % forces expressed in link coordinates; so f_ext has to be transformed to 17 | % link coordinates before use. The arguments parent and Xup contain the 18 | % parent array and link-to-link coordinate transforms for the model to 19 | % which the forces apply, and are used to work out the coordinate 20 | % transforms. 21 | 22 | % Note: the possibility exists to allow various formats for f_ext; 23 | % e.g. 6/3xNB matrix, or structure with shortened cell array f_ext and a 24 | % list of body numbers (f_ext{i} applies to body b(i)) 25 | 26 | f_out = f_in; 27 | 28 | if length(f_ext) > 0 29 | for i = 1:length(parent) 30 | if parent(i) == 0 31 | Xa{i} = Xup{i}; 32 | else 33 | Xa{i} = Xup{i} * Xa{parent(i)}; 34 | end 35 | if length(f_ext{i}) > 0 36 | f_out{i} = f_out{i} - Xa{i}' \ f_ext{i}; 37 | end 38 | end 39 | end 40 | -------------------------------------------------------------------------------- /dynamics/fbanim.m: -------------------------------------------------------------------------------- 1 | function Q = fbanim( X, Qr ) 2 | 3 | % fbanim Floating Base Inverse Kinematics for Animation 4 | % Q=fbanim(X) and Q=fbanim(X,Qr) calculate matrices of joint position data 5 | % for use in showmotion animations. Q=fbanim(X) calculates a 6xN matrix Q 6 | % from a 13x1xN or 13xN or 7x1xN or 7xN matrix X, such that each column of 7 | % X contains at least the first 7 elements of a 13-element singularity-free 8 | % state vector used by FDfb and IDfb, and the corresponding column of Q 9 | % contains the position variables of the three prismatic and three revolute 10 | % joints that form the floating base. Each column of Q is calculated using 11 | % fbkin; but fbanim then adjusts the three revolute joint variables to 12 | % remove the pi and 2*pi jumps that occur when the angles wrap around or 13 | % pass through a kinematic singularity. As a result, some or all of the 14 | % revolute joint angles may grow without limit. This is essential for a 15 | % smooth animation. Q=fbanim(X,Qr) performs the same calculation as just 16 | % described, but then appends Qr to Q. Qr must be an MxN or Mx1xN matrix 17 | % of joint position data for the real joints in a floating-base mechanism, 18 | % and the resulting (6+M)xN matrix Q contains the complete set of joint 19 | % data required by showmotion. Note: the algorithm used to remove the 20 | % jumps makes continuity assumptions (specifically, less than pi/2 changes 21 | % from one column to the next, except when passing through a singularity) 22 | % that are not guaranteed to be true. Therefore, visible glitches in an 23 | % animation are still possible. 24 | 25 | if length(size(X)) == 3 % collapse 3D -> 2D array 26 | tmp(:,:) = X(:,1,:); 27 | X = tmp; 28 | end 29 | 30 | % apply kinematic transform using fbkin 31 | 32 | for i = 1 : size(X,2) 33 | Q(:,i) = fbkin( X(1:7,i) ); 34 | end 35 | 36 | % This code removes wrap-arounds and step-changes on passing through a 37 | % singularity. Whenever q6 or q4 wrap, they jump by 2*pi. However, when 38 | % q5 hits pi/2 (or -pi/2), q4 and q6 both jump by pi, and q5 turns around. 39 | % To undo this, the code looks to see whether n is an even or odd number, 40 | % and replaces q5 with pi-q5 whenever n is odd. q4 is calculated via the 41 | % sum or difference of q4 and q6 on the grounds that the sum well defined 42 | % at the singularity at q5=pi/2 and the difference is well defined at 43 | % q5=-pi/2. 44 | 45 | for i = 2 : size(X,2) 46 | n = round( (Q(6,i-1) - Q(6,i)) / pi ); 47 | q6 = Q(6,i) + n*pi; 48 | if Q(5,i) >= 0 49 | q46 = Q(4,i) + Q(6,i); 50 | q46 = q46 + 2*pi * round( (Q(4,i-1)+Q(6,i-1) - q46) / (2*pi) ); 51 | Q(4,i) = q46 - q6; 52 | else 53 | q46 = Q(4,i) - Q(6,i); 54 | q46 = q46 + 2*pi * round( (Q(4,i-1)-Q(6,i-1) - q46) / (2*pi) ); 55 | Q(4,i) = q46 + q6; 56 | end 57 | Q(6,i) = q6; 58 | if mod(n,2) == 0 59 | q5 = Q(5,i); 60 | else 61 | q5 = pi - Q(5,i); 62 | end 63 | Q(5,i) = q5 + 2*pi * round( (Q(5,i-1) - q5) / (2*pi) ); 64 | end 65 | 66 | % add the rest of the joint data, if argument Qr has been supplied 67 | 68 | if nargin == 2 69 | if length(size(Qr)) == 3 % collapse 3D -> 2D array 70 | tmp = zeros(size(Qr,1),size(Qr,3)); 71 | tmp(:,:) = Qr(:,1,:); 72 | Qr = tmp; 73 | end 74 | Q = [ Q; Qr ]; 75 | end 76 | -------------------------------------------------------------------------------- /dynamics/get_gravity.m: -------------------------------------------------------------------------------- 1 | function a_grav = get_gravity( model ) 2 | 3 | % get_gravity spatial/planar gravitational accn vector for given model 4 | % get_gravity(model) returns the gravitational acceleration vector to be 5 | % used in dynamics calculations for the given model. The return value is 6 | % either a spatial or a planar vector, according to the type of model. It 7 | % is computed from the field model.gravity, which is a 2D or 3D (row or 8 | % column) vector specifying the linear acceleration due to gravity. If 9 | % this field is not present then get_gravity uses the following defaults: 10 | % [0,0,-9.81] for spatial models and [0,0] for planar. 11 | 12 | if isfield( model, 'gravity' ) 13 | g = model.gravity; 14 | else 15 | g = [0;0;-9.81]; 16 | end 17 | 18 | if size(model.Xtree{1},1) == 3 % is model planar? 19 | a_grav = [0;g(1);g(2)]; 20 | else 21 | a_grav = [0;0;0;g(1);g(2);g(3)]; 22 | end 23 | -------------------------------------------------------------------------------- /models/autoTree.m: -------------------------------------------------------------------------------- 1 | function model = autoTree( nb, bf, skew, taper ) 2 | 3 | % autoTree Create System Models of Kinematic Trees 4 | % autoTree(nb,bf,skew,taper) creates system models of kinematic trees 5 | % having revolute joints, mainly for the purpose of testing dynamics 6 | % functions. nb and bf specify the number of bodies in the tree, and the 7 | % branching factor, respectively. The latter is the average number of 8 | % children of a nonterminal node, and must be >=1. bf=1 produces an 9 | % unbranched tree; bf=2 produces a binary tree; and non-integer values 10 | % produce trees in which the number of children alternates between 11 | % floor(bf) and ceil(bf) in such a way that the average is bf. Trees are 12 | % constructed (and numbered) breadth-first. Link i is a thin-walled 13 | % cylindrical tube of length l(i), radius l(i)/20, and mass m(i), lying 14 | % between 0 and l(i) on the x axis of its local coordinate system. The 15 | % values of l(i) and m(i) are determined by the tapering coefficient: 16 | % l(i)=taper^(i-1) and m(i)=taper^(3*(i-1)). Thus, if taper=1 then 17 | % m(i)=l(i)=1 for all i. The inboard joint axis of link i lies on the 18 | % local z axis, and its outboard axis passes through the point (l(i),0,0) 19 | % and is rotated about the x axis by an angle of skew radians relative to 20 | % the inboard axis. If the link has more than one outboard joint then they 21 | % all have the same axis. If skew=0 then the mechanism is planar. The 22 | % final one, two or three arguments can be omitted, in which case they 23 | % assume default values of taper=1, skew=0 and bf=1. 24 | 25 | if nargin < 4 26 | taper = 1; 27 | end 28 | if nargin < 3 29 | skew = 0; 30 | end 31 | if nargin < 2 32 | bf = 1; 33 | end 34 | 35 | model.NB = nb; 36 | 37 | for i = 1:nb 38 | model.jtype{i} = 'R'; 39 | model.parent(i) = floor((i-2+ceil(bf))/bf); 40 | if model.parent(i) == 0 41 | model.Xtree{1} = xlt([0 0 0]); 42 | else 43 | model.Xtree{i} = rotx(skew) * xlt([len(model.parent(i)),0,0]); 44 | end 45 | len(i) = taper^(i-1); 46 | mass = taper^(3*(i-1)); 47 | CoM = len(i) * [0.5,0,0]; 48 | Icm = mass * len(i)^2 * diag([0.0025,1.015/12,1.015/12]); 49 | model.I{i} = mcI( mass, CoM, Icm ); 50 | end 51 | 52 | % drawing instructions 53 | 54 | model.appearance.base = ... 55 | { 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.06] }; 56 | 57 | p0 = -1; 58 | for i = 1:nb 59 | p1 = model.parent(i); 60 | tap = taper^(i-1); 61 | if p1 == 0 62 | ptap = 1; 63 | else 64 | ptap = taper^(p1-1); 65 | end 66 | if ( p1 > p0 ) 67 | model.appearance.body{i} = ... 68 | { 'cyl', [0 0 0; 1 0 0]*tap, 0.05*tap, ... 69 | 'cyl', [0 0 -0.07; 0 0 0.07]*ptap, 0.08*ptap }; 70 | p0 = p1; 71 | else 72 | model.appearance.body{i} = ... 73 | { 'cyl', [0 0 0; 1 0 0]*tap, 0.05*tap }; 74 | end 75 | end 76 | -------------------------------------------------------------------------------- /models/doubleElbow.m: -------------------------------------------------------------------------------- 1 | function robot = doubleElbow 2 | 3 | % doubleElbow planar robot with geared 2R elbow joint (Simulink Example 6) 4 | % doubleElbow creates a two-link planar robot that resembles planar(2), 5 | % except that the elbow joint has been replaced by a pair of revolute 6 | % joints close together, which are geared 1:1. This kind of joint can be 7 | % implemented with a pair of gears, or a pair of pulleys and a figure-eight 8 | % cable. This robot therefore has three revolute joints in total, but only 9 | % two independent degrees of fredom. The gearing constraint is implemented 10 | % by a constraint function provided in gamma_q. This robot is used in 11 | % Simulink Example 6. 12 | 13 | % For efficient use with Simulink, this function creates a model data 14 | % structure the first time it is called, and thereafter returns the stored 15 | % value below. 16 | 17 | persistent memory; 18 | 19 | if length(memory) ~= 0 20 | robot = memory; 21 | return 22 | end 23 | 24 | % This robot is implemented as a 3-link chain with 3 revolute joints, 25 | % but the joints are subject to the constraint q(2)==q(3). 26 | 27 | robot.NB = 3; 28 | robot.parent = [0 1 2]; 29 | robot.jtype = { 'r', 'r', 'r' }; 30 | 31 | robot.gamma_q = @gamma_q; % constraint-imposing function 32 | 33 | robot.Xtree = { eye(3), plnr(0,[1,0]), plnr(0,[0.25,0]) }; 34 | 35 | Ilink = mcI(1,[0.5,0],1/12); % inertia of the two main links 36 | 37 | robot.I = { Ilink, zeros(3), Ilink }; 38 | 39 | robot.appearance.base = ... 40 | { 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.07] }; 41 | 42 | robot.appearance.body{1} = ... 43 | { 'box', [0 -0.07 -0.04; 1 0.07 0.04], ... 44 | 'cyl', [0 0 -0.07; 0 0 0.07], 0.1, ... 45 | 'cyl', [1 0 -0.05; 1 0 0.05], 0.125 }; 46 | 47 | robot.appearance.body{2} = ... 48 | { 'box', [0 -0.06 0.05; 0.25 0.06 0.08], ... 49 | 'box', [0 -0.06 -0.05; 0.25 0.06 -0.08], ... 50 | 'cyl', [0 0 -0.08; 0 0 0.08], 0.06, ... 51 | 'cyl', [0.25 0 -0.08; 0.25 0 0.08], 0.06 }; 52 | 53 | robot.appearance.body{3} = ... 54 | { 'box', [0 -0.07 -0.04; 1 0.07 0.04], ... 55 | 'cyl', [0 0 -0.05; 0 0 0.05], 0.125 }; 56 | 57 | 58 | % The function below implements the constraint q(2)==q(3). If qo and qdo 59 | % satisfy the constraints exactly then they are returned as q and qd. 60 | % Otherwise, qo(2) and qdo(2) are taken as correct, qo(3) and qdo(3) as 61 | % erroneous, and the returned value of gs includes a constraint 62 | % stabilization term that causes qo(3) and qdo(3) to converge towards qo(2) 63 | % and qdo(2) as the simulation proceeds. 64 | 65 | function [q,qd,G,gs] = gamma_q( model, qo, qdo ) 66 | 67 | q = [ qo(1); qo(2); qo(2) ]; % satisfies constraint exactly 68 | 69 | qd = [ qdo(1); qdo(2); qdo(2) ]; % satisfies constraint exactly 70 | 71 | G = [ 1 0; 0 1; 0 1 ]; 72 | 73 | Tstab = 0.1; 74 | 75 | gs = 2/Tstab*(qd-qdo) + 1/Tstab^2*(q-qo); 76 | -------------------------------------------------------------------------------- /models/floatbase.m: -------------------------------------------------------------------------------- 1 | function fbmodel = floatbase( model ) 2 | 3 | % floatbase construct the floating-base equivalent of a fixed-base model 4 | % floatbase(model) converts a fixed-base spatial kinematic tree to a 5 | % floating-base kinematic tree as follows: old body 1 becomes new body 6, 6 | % and is regarded as the floating base; old joint 1 is discarded; six new 7 | % joints are added (three prismatic and three revolute, in that order, 8 | % arranged along the x, y and z axes, in that order); and five new 9 | % zero-mass bodies are added (numbered 1 to 5), to connect the new joints. 10 | % All other bodies and joints in the given model are preserved, but their 11 | % identification numbers are incremented by 5. The end result is that body 12 | % 6 has a full 6 degrees of motion freedom relative to the fixed base, with 13 | % joint variables 1 to 6 serving as a set of 3 Cartesian coordinates and 3 14 | % rotation angles (about x, y and z axes) specifying the position and 15 | % orientation of body 6's coordinate frame relative to the base coordinate 16 | % frame. CAUTION: A singularity occurs when q(5)=+-pi/2. If this is a 17 | % problem then use the special functions FDfb and IDfb instead of the 18 | % standard dynamics functions. floatbase requires that old joint 1 is the 19 | % only joint connected to the fixed base, and that Xtree{1} is the 20 | % identity. 21 | 22 | if size(model.Xtree{1},1) == 3 23 | error('floatbase applies to spatial models only'); 24 | end 25 | 26 | if any( model.parent(2:model.NB) == 0 ) 27 | error('only one connection to a fixed base allowed'); 28 | end 29 | 30 | if isfield( model, 'gamma_q' ) 31 | warning('floating a model with gamma_q (joint numbers will change)'); 32 | end 33 | 34 | if ~isequal( model.Xtree{1}, eye(6) ) 35 | warning('Xtree{1} not identity'); 36 | end 37 | 38 | fbmodel = model; 39 | 40 | fbmodel.NB = model.NB + 5; 41 | 42 | fbmodel.jtype = ['Px' 'Py' 'Pz' 'Rx' 'Ry' 'Rz' model.jtype(2:end)]; 43 | 44 | fbmodel.parent = [0 1 2 3 4 model.parent+5]; 45 | 46 | fbmodel.Xtree = [eye(6) eye(6) eye(6) eye(6) eye(6) model.Xtree]; 47 | 48 | fbmodel.I = [zeros(6) zeros(6) zeros(6) zeros(6) zeros(6) model.I]; 49 | 50 | if isfield( model, 'appearance' ) 51 | fbmodel.appearance.body = {{}, {}, {}, {}, {}, model.appearance.body{:}}; 52 | end 53 | 54 | if isfield( model, 'camera' ) && isfield( model.camera, 'body' ) 55 | if model.camera.body > 0 56 | fbmodel.camera.body = model.camera.body + 5; 57 | end 58 | end 59 | 60 | if isfield( model, 'gc' ) 61 | fbmodel.gc.body = model.gc.body + 5; 62 | end 63 | -------------------------------------------------------------------------------- /models/planar.m: -------------------------------------------------------------------------------- 1 | function robot = planar( n ) 2 | 3 | % planar create an n-link planar robot. 4 | % planar(n) creates an all-revolute n-link planar robot with identical 5 | % links. For efficient use with Simulink, this function stores a copy of 6 | % the last robot it created, and returns the copy if the arguments match. 7 | 8 | persistent last_robot last_n; 9 | 10 | if length(last_robot) ~= 0 && last_n == n 11 | robot = last_robot; 12 | return 13 | end 14 | 15 | robot.NB = n; 16 | robot.parent = [0:n-1]; 17 | 18 | for i = 1:n 19 | robot.jtype{i} = 'r'; 20 | if i == 1 21 | robot.Xtree{i} = plnr( 0, [0 0]); 22 | else 23 | robot.Xtree{i} = plnr( 0, [1 0]); 24 | end 25 | robot.I{i} = mcI( 1, [0.5 0], 1/12 ); 26 | end 27 | 28 | robot.appearance.base = ... 29 | { 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.07] }; 30 | 31 | for i = 1:n 32 | robot.appearance.body{i} = ... 33 | { 'box', [0 -0.07 -0.04; 1 0.07 0.04], ... 34 | 'cyl', [0 0 -0.07; 0 0 0.07], 0.1 }; 35 | end 36 | 37 | last_robot = robot; 38 | last_n = n; 39 | -------------------------------------------------------------------------------- /models/rollingdisc.m: -------------------------------------------------------------------------------- 1 | function model = rollingdisc( npt ) 2 | 3 | % rollingdisc disc with ground contact points (Simulink Example 7) 4 | % rollingdisc(npt) creates a model data structure for a polyhedral 5 | % approximation to a thick circular disc (i.e., a cylinder) that can make 6 | % contact with, bounce, and roll over the ground (the x-y plane). The 7 | % argument is the number of vertices in the polygon approximating a circle. 8 | % The model contains 2*npt ground-contact points -- npt around each end 9 | % face of the cylinder. See Simulink Example 7. 10 | 11 | % For efficient use with Simulink, this function builds a new model only 12 | % if it doesn't have a stored copy of the right model. 13 | 14 | persistent last_model last_npt; 15 | 16 | if length(last_model) ~= 0 && last_npt == npt 17 | model = last_model; 18 | return 19 | end 20 | 21 | model.NB = 1; 22 | model.parent = 0; 23 | 24 | model.jtype = {'R'}; % sacrificial joint replaced by floatbase 25 | model.Xtree = {eye(6)}; 26 | 27 | % Disc dimensions and inertia 28 | 29 | R = 0.05; % radius 30 | T = 0.01; % thickness 31 | 32 | density = 3000; % kg / m^3 33 | mass = pi * R^2 * T * density; 34 | Ibig = mass * R^2 / 2; 35 | Ismall = mass * (3*R^2 + T^2) / 12; 36 | 37 | model.I = { mcI( mass, [0 0 0], diag([Ismall Ibig Ismall]) ) }; 38 | 39 | % Drawing instructions 40 | 41 | model.appearance.base = { 'tiles', [-0.1 1; -0.3 0.3; 0 0], 0.1 }; 42 | 43 | model.appearance.body{1} = { 'facets', npt, 'cyl', [0 -T/2 0; 0 T/2 0], R }; 44 | 45 | % Camera settings 46 | 47 | model.camera.body = 1; 48 | model.camera.direction = [1 0 0.1]; 49 | model.camera.locus = [0 0.5]; 50 | 51 | % Ground contact points (have to match how the cylinder is drawn) 52 | 53 | ang = (0:npt-1) * 2*pi / npt; 54 | 55 | Y = ones(1,npt) * T/2; 56 | X = sin(ang) * R; 57 | Z = cos(ang) * R; 58 | 59 | model.gc.point = [ X X; -Y Y; Z Z ]; 60 | model.gc.body = ones(1,2*npt); 61 | 62 | % Final step: float the base 63 | 64 | model = floatbase(model); % replace joint 1 with a chain of 6 65 | % joints emulating a floating base 66 | last_model = model; 67 | last_npt = npt; 68 | -------------------------------------------------------------------------------- /models/scissor.m: -------------------------------------------------------------------------------- 1 | function model = scissor 2 | 3 | % SCISSOR free-floating two-body system resembling a pair of scissors 4 | % scissor creates a model data structure for a free-floating two body 5 | % system consisting of two identical flat rectangular bodies connected 6 | % together at the middle by a revolute joint in a manner resembling a pair 7 | % of scissors. In the zero position, both bodies extend from -0.1 to 0.1 8 | % in the x direction, and from -0.5 to 0.5 in the z direction, but one 9 | % extends from 0 to -0.1 and the other from 0 to +0.1 in the y direction, 10 | % and the connecting joint lies on the y axis. Both bodies have unit 11 | % mass. This mechanism is used in Simulink Example 5. 12 | 13 | % For efficient use with Simulink, this function builds a new model only 14 | % the first time it is called. Thereafter, it returns this stored copy. 15 | 16 | persistent memory; 17 | 18 | if length(memory) ~= 0 19 | model = memory; 20 | return 21 | end 22 | 23 | model.NB = 2; 24 | model.parent = [0 1]; 25 | 26 | model.jtype = {'R', 'Ry'}; % 1st joint will be replaced by floatbase 27 | model.Xtree = {eye(6), eye(6)}; 28 | 29 | model.gravity = [0 0 0]; % zero gravity is not the default, 30 | % so it must be stated explicitly 31 | 32 | % centroidal inertia of a unit-mass box with dimensions 0.2, 0.1 and 1: 33 | 34 | Ic = diag( [1.01 1.04 0.05] / 12 ); 35 | 36 | model.I = { mcI(1,[0 -0.05 0],Ic), mcI(1,[0 0.05 0],Ic) }; 37 | 38 | % Draw the bodies 39 | 40 | model.appearance.body{1} = ... 41 | { 'box', [-0.1 -0.1 -0.5; 0.1 0 0.5], ... 42 | 'colour', [0.6 0.3 0.8], ... 43 | 'cyl', [0 -0.13 0; 0 0.13 0], 0.07 }; 44 | 45 | model.appearance.body{2} = ... 46 | { 'box', [-0.1 0 -0.5; 0.1 0.1 0.5] }; 47 | 48 | model.camera.direction = [0 -3 1]; % a better viewing angle 49 | model.camera.zoom = 0.9; 50 | 51 | model = floatbase(model); % replace joint 1 with a chain of 6 52 | % joints emulating a floating base 53 | memory = model; 54 | -------------------------------------------------------------------------------- /models/singlebody.m: -------------------------------------------------------------------------------- 1 | function model = singlebody 2 | 3 | % SINGLEBODY free-floating single rigid body 4 | % singlebody creates a model data structure for a single free-floating, 5 | % unit-mass rigid body having the shape of a rectangular box with 6 | % dimensions of 3, 2 and 1 in the x, y and z directions, respectively. 7 | 8 | % For efficient use with Simulink, this function builds a new model only 9 | % the first time it is called. Thereafter, it returns this stored copy. 10 | 11 | persistent memory; 12 | 13 | if length(memory) ~= 0 14 | model = memory; 15 | return 16 | end 17 | 18 | model.NB = 1; 19 | model.parent = 0; 20 | 21 | model.jtype = {'R'}; % sacrificial joint replaced by floatbase 22 | model.Xtree = {eye(6)}; 23 | 24 | model.gravity = [0 0 0]; % zero gravity is not the default, 25 | % so it must be stated explicitly 26 | 27 | % This is the inertia of a uniform-density box with one vertex at (0,0,0) 28 | % and a diametrically opposite one at (3,2,1). 29 | 30 | model.I = { mcI( 1, [3 2 1]/2, diag([4+1 9+1 9+4]/12) ) }; 31 | 32 | % Draw the global X, Y and Z axes in red, green and blue 33 | 34 | model.appearance.base = ... 35 | { 'colour', [0.9 0 0], 'line', [0 0 0; 2 0 0], ... 36 | 'colour', [0 0.9 0], 'line', [0 0 0; 0 2 0], ... 37 | 'colour', [0 0.3 0.9], 'line', [0 0 0; 0 0 2] }; 38 | 39 | % Draw the floating body 40 | 41 | model.appearance.body{1} = { 'box', [0 0 0; 3 2 1] }; 42 | 43 | model = floatbase(model); % replace joint 1 with a chain of 6 44 | % joints emulating a floating base 45 | memory = model; 46 | -------------------------------------------------------------------------------- /simulink/ex4_anal.m: -------------------------------------------------------------------------------- 1 | % Analysis of Drift in Conserved Quantities in Simulink Example 4 2 | % --------------------------------------------------------------- 3 | % 4 | % It is assumed that Example 4 has already been run, and that there is 5 | % therefore a variable called tout, and another called xout, containing the 6 | % results of a simulation run. This script calculates and plots drift in 7 | % kinetic energy, position and velocity of centre of mass, and spatial 8 | % momentum. Potential energy is always exactly zero because gravity is 9 | % zero. 10 | 11 | N = length(tout); 12 | 13 | for i = 1:N 14 | [q,qd] = fbkin( xout(:,1,i) ); 15 | ret = EnerMo( singlebody, q, qd ); 16 | KE(i) = ret.KE; 17 | cm(:,i) = ret.cm; 18 | vcm(:,i) = ret.vcm; 19 | h(:,i) = ret.htot; 20 | end 21 | 22 | KEdrift = KE - KE(1); 23 | cmdrift = cm - cm(:,1) * ones(1,N); 24 | vcmdrift = vcm - vcm(:,1) * ones(1,N); 25 | hdrift = h - h(:,1) * ones(1,N); 26 | 27 | plot( tout, KEdrift ); 28 | title( 'Drift in Kinetic Energy' ); 29 | 30 | figure; 31 | plot( tout, [cmdrift; vcmdrift] ); 32 | title( 'Drift in Centre of Mass Location and Velocity' ); 33 | legend('x', 'y', 'z', 'x vel', 'y vel', 'z vel', 'Location', 'NorthWest'); 34 | 35 | figure; 36 | plot( tout, hdrift ); 37 | title( 'Drift in Spatial Momentum' ); 38 | legend('ang x', 'ang y', 'ang z', 'lin x', 'lin y', 'lin z', ... 39 | 'Location', 'NorthWest'); 40 | -------------------------------------------------------------------------------- /simulink/gcFD.m: -------------------------------------------------------------------------------- 1 | function y = gcFD( model, FDfun, u ) 2 | 3 | % gcFD Simulink wrapper for forward dynamics functions 4 | % y=gcFD(model,FDfun,u) is a Simulink wrapper for the forward-dynamics 5 | % functions FDab, FDcrb, FDfb and FDgq. It serves two purposes: (1) to 6 | % accept ground contact forces in the format produced by the spatial_v2 7 | % Simulink ground model and convert them into the format required by the 8 | % dynamics functions, and (2) to concatenate into a single vector the two 9 | % return values of FDfb. The first argument is the model data structure; 10 | % the second is the function handle of the dynamics function to be used; 11 | % and the third is the concatenation of the vectors q, qd, tau and 12 | % (optionally) the ground contact force data (f_gc) from which f_ext can 13 | % be calculated. (For FDfb, u is the concatenation of x, q, qd, tau and 14 | % optional f_gc.) The ground contact force data, if supplied, is simply 15 | % the concatenation of the spatial (or planar) forces arising from each 16 | % point defined in model.gc.point, all expressed in base coordinates. 17 | 18 | N = model.NB; 19 | 20 | if isequal( FDfun, @FDfb ) 21 | x = u(1:13); 22 | q = u(14:N+7); 23 | qd = u(N+8:2*N+1); 24 | tau = u(2*N+2:3*N-5); 25 | f_gc = u(3*N-4:end); 26 | else 27 | q = u(1:N); 28 | qd = u(N+1:2*N); 29 | tau = u(2*N+1:3*N); 30 | f_gc = u(3*N+1:end); 31 | end 32 | 33 | if length(f_gc) > 0 34 | vecsize = length(model.Xtree{1}); % 3 for planar, 6 for spatial 35 | b = model.gc.body; % b(i) == body number of point i 36 | f_gc = reshape( f_gc, vecsize, length(b) ); 37 | f_ext = cell(1,N); 38 | % for each body i that is mentioned at least once, calculate the sum of 39 | % all forces acting on that body 40 | for i = unique(b) 41 | f_ext{i} = sum( f_gc(:,b==i), 2 ); 42 | end 43 | else 44 | f_ext = {}; 45 | end 46 | 47 | if isequal( FDfun, @FDfb ) 48 | [xd, qdd] = FDfb( model, x, q, qd, tau, f_ext ); 49 | y = [xd; qdd]; 50 | else 51 | y = FDfun( model, q, qd, tau, f_ext ); 52 | end 53 | -------------------------------------------------------------------------------- /simulink/gcPosVel.m: -------------------------------------------------------------------------------- 1 | function posvel = gcPosVel( model, xfb, q, qd ) 2 | 3 | % gcPosVel calculate positions and velocities of contact points 4 | % gcPosVel(model,q,qd), gcPosVel(model,xfb,q,qd) and gcPosVel(model,xfb) 5 | % calculate a 4xn or 6xn matrix, depending on whether the model is planar 6 | % or spatial, containing the position and linear velocity of every point 7 | % specified in model.gc.point (n==length(model.gc.point)). The position 8 | % coordinates appear in the top rows, and the velocity coordinates at the 9 | % bottom. All are expressed in base (i.e., absolute) coordinates. The 10 | % data for model.gc.point(i) appears in column i. The argument are: the 11 | % model data structure (which must contain a .gc substructure); the 12 | % position and velocity variables of a floating base (as defined by FDfb 13 | % and IDfb); the joint position variables; and the joint velocity 14 | % variables. The call gcPosVel(model,xfb) is permissible only if the 15 | % model describes a single floating rigid body. 16 | 17 | if nargin==4 || nargin==2 % xfb supplied 18 | 19 | qn = xfb(1:4); % unit quaternion fixed-->f.b. 20 | r = xfb(5:7); % position of f.b. origin 21 | Xa{6} = plux( rq(qn), r ); % xform fixed --> f.b. coords 22 | 23 | vfb = xfb(8:end); 24 | vb{6} = Xa{6} * vfb; % f.b. vel in f.b. coords 25 | 26 | for i = 7:model.NB 27 | [ XJ, S ] = jcalc( model.jtype{i}, q(i-6) ); 28 | Xup = XJ * model.Xtree{i}; 29 | vJ = S*qd(i-6); 30 | Xa{i} = Xup * Xa{model.parent(i)}; 31 | vb{i} = Xup * vb{model.parent(i)} + vJ; 32 | end 33 | 34 | else % xfb not supplied 35 | 36 | qd = q; q = xfb; % shift up the arguments 37 | 38 | for i = 1:model.NB 39 | [ XJ, S ] = jcalc( model.jtype{i}, q(i) ); 40 | Xup = XJ * model.Xtree{i}; 41 | vJ = S*qd(i); 42 | if model.parent(i) == 0 43 | Xa{i} = Xup; 44 | vb{i} = vJ; 45 | else 46 | Xa{i} = Xup * Xa{model.parent(i)}; 47 | vb{i} = Xup * vb{model.parent(i)} + vJ; 48 | end 49 | end 50 | 51 | end 52 | 53 | for i = unique(model.gc.body) 54 | X = inv(Xa{i}); % xform body i -> abs coords 55 | v = X * vb{i}; % body i vel in abs coords 56 | iset = model.gc.body == i; % set of points assoc with body i 57 | pt = Xpt( X, model.gc.point(:,iset) ); % xform points to abs coords 58 | vpt = Vpt( v, pt ); % linear velocities of points 59 | posvel(:,iset) = [ pt; vpt ]; % insert into correct columns 60 | end 61 | -------------------------------------------------------------------------------- /sparsity/LTDL.m: -------------------------------------------------------------------------------- 1 | function [L,D] = LTDL( H, lambda ) 2 | 3 | % LTDL factorize H -> L'*D*L exploiting branch-induced sparsity 4 | % [L,D]=LTDL(H,lambda) returns a unit-lower-triangular matrix L and a 5 | % diagonal matrix D that satisfy L'*D*L = H, where H is a symmetric, 6 | % positive-definite matrix having the property that the nonzero elements on 7 | % row i below the main diagonal appear only in columns lambda(i), 8 | % lambda(lambda(i)), and so on. This is the pattern of branch-induced 9 | % sparsity; and H and lambda can be regarded as the joint-space inertia 10 | % matrix and parent array of a kinematic tree. lambda must satisfy 11 | % 0<=lambda(i) L'*L exploiting branch-induced sparsity 4 | % LTL(H,lambda) returns a lower-triangular matrix L satisfying L'*L = H, 5 | % where H is a symmetric, positive-definite matrix having the property that 6 | % the nonzero elements on row i below the main diagonal appear only in 7 | % columns lambda(i), lambda(lambda(i)), and so on. This is the pattern of 8 | % branch-induced sparsity; and H and lambda can be regarded as the 9 | % joint-space inertia matrix and parent array of a kinematic tree. lambda 10 | % must satisfy 0<=lambda(i) spatial/planar forces 4 | % f=Fpt(fp,p) converts one or more linear forces fp acting at one or more 5 | % points p to their equivalent spatial or planar forces. In 3D, fp and p 6 | % are 3xn arrays and f is 6xn. In 2D, fp and p are 2xn and f is 3xn. In 7 | % both cases, f(:,i) is the equivalent of fp(:,i) acting at p(:,i). 8 | 9 | if size(fp,1)==3 % 3D forces at 3D points 10 | f = [ cross(p,fp,1); fp ]; 11 | else % 2D forces at 2D points 12 | f = [ p(1,:).*fp(2,:) - p(2,:).*fp(1,:); fp ]; 13 | end 14 | -------------------------------------------------------------------------------- /spatial/Vpt.m: -------------------------------------------------------------------------------- 1 | function vp = Vpt( v, p ) 2 | 3 | % Vpt spatial/planar velocities --> velocities at points 4 | % vp=Vpt(v,p) calculates the linear velocities vp at one or more points p 5 | % due to one or more spatial/planar velocities v. In 3D, v is either 6x1 6 | % or 6xn, and p and vp are 3xn. In 2D, v is either 3x1 or 3xn, and p and 7 | % vp are 2xn. If v is just a single spatial/planar vector then it applies 8 | % to every point in p; otherwise, vp(:,i) is calculated from v(:,i) and 9 | % p(:,i). 10 | 11 | if size(v,2)==1 && size(p,2)>1 12 | v = repmat(v,1,size(p,2)); 13 | end 14 | 15 | if size(v,1)==6 % 3D points and velocities 16 | vp = v(4:6,:) + cross(v(1:3,:),p,1); 17 | else % 2D points and velocities 18 | vp = v(2:3,:) + [ -v(1,:).*p(2,:); v(1,:).*p(1,:) ]; 19 | end 20 | -------------------------------------------------------------------------------- /spatial/Xpt.m: -------------------------------------------------------------------------------- 1 | function xp = Xpt( X, p ) 2 | 3 | % Xpt apply Plucker/planar coordinate transform to 2D/3D points 4 | % xp=Xpt(X,p) applies the coordinate transform X to the points in p, 5 | % returning the new coordinates in xp. If X is a 6x6 matrix then it is 6 | % taken to be a Plucker coordinate transform, and p is expected to be a 7 | % 3xn matrix of 3D points. Otherwise, X is assumed to be a planar 8 | % coordinate transform and p a 2xn array of 2D points. 9 | 10 | if all(size(X)==[6 6]) % 3D points 11 | E = X(1:3,1:3); 12 | r = -skew(E'*X(4:6,1:3)); 13 | else % 2D points 14 | E = X(2:3,2:3); 15 | r = [ X(2,3)*X(2,1)+X(3,3)*X(3,1); X(2,3)*X(3,1)-X(3,3)*X(2,1) ]; 16 | end 17 | 18 | if size(p,2) > 1 19 | r = repmat(r,1,size(p,2)); 20 | end 21 | 22 | xp = E * (p - r); 23 | -------------------------------------------------------------------------------- /spatial/XtoV.m: -------------------------------------------------------------------------------- 1 | function v = XtoV( X ) 2 | 3 | % XtoV obtain spatial/planar vector from small-angle transform. 4 | % XtoV(X) interprets X as the coordinate transform from A to B 5 | % coordinates, which implicitly defines the location of frame B relative to 6 | % frame A. XtoV calculates the velocity of a third frame, C(t), that 7 | % travels at constant velocity from frame A to frame B in one time unit. 8 | % Thus, C(0)=A, C(1)=B and dC/dt is a constant. The return value, v, is 9 | % the velocity of C, calculated using a small-angle approximation. It is 10 | % therefore exact only if A and B are parallel. The return value is a 11 | % spatial vector if X is a 6x6 matrix; otherwise it is a planar vector. 12 | % The return value is an invariant of X (i.e., v=X*v), and can therefore be 13 | % regarded as being expressed in both A and B coordinates. 14 | 15 | if all(size(X)==[6 6]) % Plucker xform -> spatial vector 16 | 17 | v = 0.5 * [ X(2,3) - X(3,2); 18 | X(3,1) - X(1,3); 19 | X(1,2) - X(2,1); 20 | X(5,3) - X(6,2); 21 | X(6,1) - X(4,3); 22 | X(4,2) - X(5,1) ]; 23 | 24 | else % planar xform -> planar vector 25 | 26 | v = [ X(2,3); 27 | (X(3,1) + X(2,2)*X(3,1) + X(2,3)*X(2,1))/2; 28 | (-X(2,1) - X(2,2)*X(2,1) + X(2,3)*X(3,1))/2 ]; 29 | end 30 | -------------------------------------------------------------------------------- /spatial/crf.m: -------------------------------------------------------------------------------- 1 | function vcross = crf( v ) 2 | 3 | % crf spatial/planar cross-product operator (force). 4 | % crf(v) calculates the 6x6 (or 3x3) matrix such that the expression 5 | % crf(v)*f is the cross product of the motion vector v with the force 6 | % vector f. If length(v)==6 then it is taken to be a spatial vector, and 7 | % the return value is a 6x6 matrix. Otherwise, v is taken to be a planar 8 | % vector, and the return value is 3x3. 9 | 10 | vcross = -crm(v).'; 11 | -------------------------------------------------------------------------------- /spatial/crm.m: -------------------------------------------------------------------------------- 1 | function vcross = crm( v ) 2 | 3 | % crm spatial/planar cross-product operator (motion). 4 | % crm(v) calculates the 6x6 (or 3x3) matrix such that the expression 5 | % crm(v)*m is the cross product of the motion vectors v and m. If 6 | % length(v)==6 then it is taken to be a spatial vector, and the return 7 | % value is a 6x6 matrix. Otherwise, v is taken to be a planar vector, and 8 | % the return value is 3x3. 9 | 10 | if length(v(:)) == 6 && size(v,2) == 1 11 | 12 | vcross = [ 0 -v(3) v(2) 0 0 0 ; 13 | v(3) 0 -v(1) 0 0 0 ; 14 | -v(2) v(1) 0 0 0 0 ; 15 | 0 -v(6) v(5) 0 -v(3) v(2) ; 16 | v(6) 0 -v(4) v(3) 0 -v(1) ; 17 | -v(5) v(4) 0 -v(2) v(1) 0 ]; 18 | 19 | elseif length(v(:)) == 3 && size(v,2) == 1 20 | 21 | vcross = [ 0 0 0 ; 22 | v(3) 0 -v(1) ; 23 | -v(2) v(1) 0 ]; 24 | else 25 | assert(all(size(v)==[6 6]),'Wrong size input to crm'); 26 | vcross = [ skew(v(1:3,1:3)) ; skew(v(4:6,1:3))]; 27 | assert(all(diag(v) == 0),'bad input to crm') 28 | end 29 | -------------------------------------------------------------------------------- /spatial/mcI.m: -------------------------------------------------------------------------------- 1 | function [o1,o2,o3] = mcI( i1, i2, i3 ) 2 | 3 | % mcI rigid-body inertia <--> mass, CoM and rotational inertia. 4 | % rbi=mcI(m,c,I) and [m,c,I]=mcI(rbi) convert between the spatial or planar 5 | % inertia matrix of a rigid body (rbi) and its mass, centre of mass and 6 | % rotational inertia about the centre of mass (m, c and I). In the spatial 7 | % case, c is 3x1, I is 3x3 and rbi is 6x6. In the planar case, c is 2x1, I 8 | % is a scalar and rbi is 3x3. In both cases, m is a scalar. When c is an 9 | % argument, it can be either a row or a column vector. If only one 10 | % argument is supplied then it is assumed to be rbi; and if it is a 6x6 11 | % matrix then it is assumed to be spatial. Otherwise, three arguments must 12 | % be supplied, and if length(c)==3 then mcI calculates a spatial inertia 13 | % matrix. NOTE: (1) mcI(rbi) requires rbi to have nonzero mass; (2) if |c| 14 | % is much larger than the radius of gyration, or the dimensions of the 15 | % inertia ellipsoid, then extracting I from rbi is numerically 16 | % ill-conditioned. 17 | 18 | if nargin == 1 19 | [o1,o2,o3] = rbi_to_mcI( i1 ); 20 | else 21 | o1 = mcI_to_rbi( i1, i2, i3 ); 22 | end 23 | 24 | 25 | function rbi = mcI_to_rbi( m, c, I ) 26 | 27 | if length(c) == 3 % spatial 28 | 29 | C = skew(c); 30 | rbi = [ I + m*C*C', m*C; m*C', m*eye(3) ]; 31 | 32 | else % planar 33 | 34 | rbi = [ I+m*dot(c,c), -m*c(2), m*c(1); 35 | -m*c(2), m, 0; 36 | m*c(1), 0, m ]; 37 | end 38 | 39 | 40 | function [m,c,I] = rbi_to_mcI( rbi ) 41 | 42 | if all(size(rbi)==[6 6]) % spatial 43 | 44 | m = rbi(6,6); 45 | mC = rbi(1:3,4:6); 46 | c = skew(mC)/m; 47 | I = rbi(1:3,1:3) - mC*mC'/m; 48 | 49 | else % planar 50 | 51 | m = rbi(3,3); 52 | c = [rbi(3,1);-rbi(2,1)]/m; 53 | I = rbi(1,1) - m*dot(c,c); 54 | 55 | end 56 | -------------------------------------------------------------------------------- /spatial/plnr.m: -------------------------------------------------------------------------------- 1 | function [o1,o2] = plnr( i1, i2 ) 2 | 3 | % plnr compose/decompose planar-vector coordinate transform. 4 | % X=plnr(theta,r) and [theta,r]=plnr(X) compose a planar-vector coordinate 5 | % transform X from its component parts theta and r, and decompose it into 6 | % those parts, respectively. theta is a scalar and r is a 2D vector. r is 7 | % returned as a column vector, but it can be supplied as a row or column 8 | % vector. X is the transform from A to B coordinates, in which frame B is 9 | % located at point r (expressed in A coordinates) and is rotated by an 10 | % angle theta (radians) relative to frame A. If two arguments are supplied 11 | % then they are assumed to be theta and r, otherwise X. 12 | 13 | if nargin == 2 % theta,r --> X 14 | 15 | c = cos(i1); 16 | s = sin(i1); 17 | 18 | o1 = [ 1 0 0 ; 19 | s*i2(1)-c*i2(2) c s ; 20 | c*i2(1)+s*i2(2) -s c ]; 21 | 22 | else % X --> theta,r 23 | 24 | c = i1(2,2); 25 | s = i1(2,3); 26 | 27 | o1 = atan2(s,c); 28 | o2 = [ s*i1(2,1)+c*i1(3,1); s*i1(3,1)-c*i1(2,1) ]; 29 | 30 | end 31 | -------------------------------------------------------------------------------- /spatial/pluho.m: -------------------------------------------------------------------------------- 1 | function out = pluho( in ) 2 | 3 | % pluho convert Plucker <--> 4x4 homogeneous coordinate transform. 4 | % X=pluho(T) and T=pluho(X) convert between a Plucker coordinate transform 5 | % matrix X and a 4x4 homogeneous coordinate transform matrix T. If the 6 | % argument is a 6x6 matrix then it is taken to be X, otherwise T. 7 | % NOTE: the 4x4 matrices used in 3D graphics (e.g. OpenGL and Matlab handle 8 | % graphics) are displacement operators, which are the inverses of 9 | % coordinate transforms. For example, to set the 'matrix' property of a 10 | % Matlab hgtransform graphics object so as to rotate its children by an 11 | % angle theta about the X axis, use inv(pluho(rotx(theta))). 12 | 13 | % Formulae: 14 | % X = [ E 0 ] T = [ E -Er ] 15 | % [ -Erx E ] [ 0 1 ] 16 | 17 | if all(size(in)==[6 6]) % Plucker -> 4x4 homogeneous 18 | E = in(1:3,1:3); 19 | mErx = in(4:6,1:3); % - E r cross 20 | out = [ E, skew(mErx*E'); 0 0 0 1 ]; 21 | else % 4x4 homogeneous -> Plucker 22 | E = in(1:3,1:3); 23 | mEr = in(1:3,4); % - E r 24 | out = [ E, zeros(3); skew(mEr)*E, E ]; 25 | end 26 | -------------------------------------------------------------------------------- /spatial/plux.m: -------------------------------------------------------------------------------- 1 | function [o1,o2] = plux( i1, i2 ) 2 | 3 | % plux compose/decompose Plucker coordinate transform. 4 | % X=plux(E,r) and [E,r]=plux(X) compose a Plucker coordinate transform X 5 | % from its component parts E and r, and decompose it into those parts, 6 | % respectively. E is a 3x3 rotational coordinate transform and r is a 3D 7 | % vector. r is returned as a column vector, but it can be supplied as a 8 | % row or column vector. X is a coordinate transform corresponding to a 9 | % shift of origin by an amount specified by r, followed by a rotation about 10 | % the new origin as specified by E. For example, plux(rx(1),[2 3 4]) makes 11 | % the same transform as rotx(1)*xlt([2 3 4]). If two arguments are 12 | % supplied then they are assumed to be E and r, otherwise X. 13 | 14 | if nargin == 2 % E,r --> X 15 | if all(size(i1) == [3 3]) 16 | o1 = [ i1, zeros(3); -i1*skew(i2), i1 ]; 17 | else 18 | o1 = [1 0 0 ; i1*[-i2(2) i2(1)].' i1]; 19 | end 20 | else 21 | X = i1; % X --> E,r 22 | if all(size(X)==[6 6]) % 3D points 23 | E = X(1:3,1:3); 24 | r = -skew(E.'*X(4:6,1:3)); 25 | else % 2D points 26 | E = X(2:3,2:3); 27 | r = [ X(2,3)*X(2,1)+X(3,3)*X(3,1); X(2,3)*X(3,1)-X(3,3)*X(2,1) ]; 28 | end 29 | o1 = E; 30 | o2 = r; 31 | end 32 | -------------------------------------------------------------------------------- /spatial/rotx.m: -------------------------------------------------------------------------------- 1 | function X = rotx( theta ) 2 | 3 | % rotx spatial coordinate transform (X-axis rotation). 4 | % rotx(theta) calculates the coordinate transform matrix from A to B 5 | % coordinates for spatial motion vectors, where coordinate frame B is 6 | % rotated by an angle theta (radians) relative to frame A about their 7 | % common X axis. 8 | 9 | c = cos(theta); 10 | s = sin(theta); 11 | 12 | X = [ 1 0 0 0 0 0 ; 13 | 0 c s 0 0 0 ; 14 | 0 -s c 0 0 0 ; 15 | 0 0 0 1 0 0 ; 16 | 0 0 0 0 c s ; 17 | 0 0 0 0 -s c 18 | ]; 19 | -------------------------------------------------------------------------------- /spatial/roty.m: -------------------------------------------------------------------------------- 1 | function X = roty( theta ) 2 | 3 | % roty spatial coordinate transform (Y-axis rotation). 4 | % roty(theta) calculates the coordinate transform matrix from A to B 5 | % coordinates for spatial motion vectors, where coordinate frame B is 6 | % rotated by an angle theta (radians) relative to frame A about their 7 | % common Y axis. 8 | 9 | c = cos(theta); 10 | s = sin(theta); 11 | 12 | X = [ c 0 -s 0 0 0 ; 13 | 0 1 0 0 0 0 ; 14 | s 0 c 0 0 0 ; 15 | 0 0 0 c 0 -s ; 16 | 0 0 0 0 1 0 ; 17 | 0 0 0 s 0 c 18 | ]; 19 | -------------------------------------------------------------------------------- /spatial/rotz.m: -------------------------------------------------------------------------------- 1 | function X = rotz( theta ) 2 | 3 | % rotz spatial coordinate transform (Z-axis rotation). 4 | % rotz(theta) calculates the coordinate transform matrix from A to B 5 | % coordinates for spatial motion vectors, where coordinate frame B is 6 | % rotated by an angle theta (radians) relative to frame A about their 7 | % common Z axis. 8 | 9 | c = cos(theta); 10 | s = sin(theta); 11 | 12 | X = [ c s 0 0 0 0 ; 13 | -s c 0 0 0 0 ; 14 | 0 0 1 0 0 0 ; 15 | 0 0 0 c s 0 ; 16 | 0 0 0 -s c 0 ; 17 | 0 0 0 0 0 1 18 | ]; 19 | -------------------------------------------------------------------------------- /spatial/xlt.m: -------------------------------------------------------------------------------- 1 | function X = xlt( r ) 2 | 3 | % xlt spatial coordinate transform (translation of origin). 4 | % xlt(r) calculates the coordinate transform matrix from A to B 5 | % coordinates for spatial motion vectors, in which frame B is translated by 6 | % an amount r (3D vector) relative to frame A. r can be a row or column 7 | % vector. 8 | 9 | X = [ 1 0 0 0 0 0 ; 10 | 0 1 0 0 0 0 ; 11 | 0 0 1 0 0 0 ; 12 | 0 r(3) -r(2) 1 0 0 ; 13 | -r(3) 0 r(1) 0 1 0 ; 14 | r(2) -r(1) 0 0 0 1 15 | ]; 16 | -------------------------------------------------------------------------------- /startup.m: -------------------------------------------------------------------------------- 1 | % When Matlab starts up, it looks for a file called startup.m in the 2 | % directory (folder) in which it is being started. If such a file is 3 | % found, then it is automatically executed as part of Matlab's start-up 4 | % procedure. The line at the end of this file tells Matlab to add the 5 | % directory '/home/users/roy/booksoft/spatial_v2', and all of its 6 | % subdirectories, to Matlab's command search path. An action like this is 7 | % necessary if Matlab is to find all of the functions in spatial_v2. 8 | 9 | % WHAT YOU MUST DO: 10 | % (1) Replace the string '/home/users/roy/booksoft/spatial_v2' with the 11 | % full path name of your directory spatial_v2 (keeping the two 12 | % single-quotes that delimit the string). For example, on a Windows 13 | % PC, if you downloaded spatial_v2.zip to the folder C:\user\john and 14 | % unzipped it there, then the replacement string will be 15 | % 'C:\user\john\spatial_v2'. 16 | % (2) Copy this file to each directory in which you want Matlab to have 17 | % access to spatial_v2 when it starts. If there is already a file 18 | % startup.m in that directory, then simply add the (edited) line 19 | % below to that file. 20 | 21 | addpath( genpath( '/home/users/roy/booksoft/spatial_v2' ) ); 22 | -------------------------------------------------------------------------------- /v3/3D/inv_skew.m: -------------------------------------------------------------------------------- 1 | function out = inv_skew( in ) 2 | 3 | out = 0.5 * [ in(3,2) - in(2,3); 4 | in(1,3) - in(3,1); 5 | in(2,1) - in(1,2) ]; 6 | -------------------------------------------------------------------------------- /v3/3D/se3toVec.m: -------------------------------------------------------------------------------- 1 | function v = se3toVec(se3) 2 | v = [skew(se3(1:3,1:3)) ; se3(1:3,4)]; 3 | end 4 | -------------------------------------------------------------------------------- /v3/3D/vecTose3.m: -------------------------------------------------------------------------------- 1 | function se3 = vecTose3(v) 2 | se3 = [skew(v(1:3)) v(4:6) ; 0 0 0 0]; 3 | end 4 | -------------------------------------------------------------------------------- /v3/derivatives/FD_derivatives.m: -------------------------------------------------------------------------------- 1 | function [dqdd_dq, dqdd_dqd,dqdd_dtau] = FD_derivatives( model, q, qd, tau ) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | 7 | qdd = FDab(model,q,qd,tau); 8 | [q, qd, qdd] = confVecToCell(model,q,qd,qdd); 9 | 10 | 11 | [dtau_dq, dtau_dqd] = ID_derivatives(model,q,qd,qdd); 12 | Hinv = Hinverse(model,q); 13 | 14 | % Relationship between FD derivatives and ID derivatives. 15 | dqdd_dtau = Hinv; 16 | dqdd_dq = -Hinv*dtau_dq; 17 | dqdd_dqd = -Hinv*dtau_dqd; 18 | -------------------------------------------------------------------------------- /v3/derivatives/H_derivatives.m: -------------------------------------------------------------------------------- 1 | function [H_derivatives] = H_derivatives( model, q) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) 7 | [q] = confVecToCell(model,q); 8 | end 9 | 10 | if sum(model.has_rotor) > 1 11 | error('H_diff does not support rotors'); 12 | end 13 | 14 | for i = 1:model.NB 15 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 16 | Xup{i} = XJ * model.Xtree{i}; 17 | end 18 | IC = model.I; % composite inertia calculation 19 | 20 | H_derivatives = repmat(0*q{1}(1),model.NV,model.NV,model.NV); 21 | 22 | for k = model.NB:-1:1 23 | for k_ind = 1:length(model.vinds{k}) 24 | kk = model.vinds{k}(k_ind); 25 | sk = S{k}(:,k_ind); 26 | 27 | Q = crf(sk)*IC{k} - IC{k}*crm(sk); % Rate of change in IC{k} due to motion of joint k 28 | Fk = IC{k}*sk; % Other term that shows up in CRBA 29 | j = k; 30 | while j > 0 31 | jj = model.vinds{j}; 32 | F1 = icrf(Fk)*S{j}; % Rate of change in Fk due to motion of joint j 33 | F2 = Q*S{j}; 34 | i = j; 35 | while i > 0 36 | ii = model.vinds{i}; 37 | if i < j 38 | H_derivatives(ii,kk,jj) = S{i}.'*F1; 39 | H_derivatives(kk,ii,jj) = S{i}.'*F1; 40 | end 41 | 42 | if j < k 43 | H_derivatives(ii,jj,kk) = S{i}.'*F2; 44 | H_derivatives(jj,ii,kk) = (S{i}.'*F2).'; 45 | end 46 | 47 | F1 = Xup{i}.'*F1; F2 = Xup{i}.'*F2; 48 | i = model.parent(i); 49 | end 50 | Fk = Xup{j}'*Fk; Q = Xup{j}'*Q*Xup{j}; 51 | j = model.parent(j); 52 | end 53 | end 54 | if model.parent(k) > 0 55 | IC{model.parent(k)} = IC{model.parent(k)} + Xup{k}.'*IC{k}*Xup{k}; 56 | end 57 | end -------------------------------------------------------------------------------- /v3/derivatives/ID_derivatives.m: -------------------------------------------------------------------------------- 1 | function [dtau_dq, dtau_dqd] = ID_derivatives( model, q, qd, qdd ) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) || ~iscell(qd) || ~iscell(qdd) 7 | [q, qd, qdd] = confVecToCell(model,q,qd,qdd); 8 | end 9 | if sum(model.has_rotor) > 1 10 | error('ID_derivatives does not support rotors'); 11 | end 12 | 13 | a_grav = get_gravity(model); 14 | IC = model.I; 15 | I = model.I; 16 | 17 | for i = 1:model.NB 18 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 19 | Xup{i} = XJ * model.Xtree{i}; 20 | 21 | if model.parent(i) == 0 22 | v{i} = zeros(6,1); 23 | a{i} = -a_grav; 24 | Xup0{i} = Xup{i}; 25 | else 26 | Xup0{i} = Xup{i}*Xup0{model.parent(i)}; 27 | v{i} = v{model.parent(i)}; 28 | a{i} = a{model.parent(i)}; 29 | end 30 | 31 | Xdown0{i} = inv(Xup0{i}); 32 | 33 | S{i} = Xdown0{i}*S{i}; 34 | vJ{i}= S{i}*qd{i}; 35 | aJ{i} = crm(v{i})*vJ{i} + S{i}*qdd{i}; 36 | 37 | Sd{i} = crm(v{i})*S{i}; 38 | Sdd{i}= crm(a{i})*S{i} + crm(v{i})*Sd{i}; 39 | Sj{i} = 2*Sd{i} + crm(vJ{i})*S{i}; 40 | 41 | v{i} = v{i} + vJ{i}; 42 | a{i} = a{i} + aJ{i}; 43 | IC{i} = Xup0{i}.'*I{i}*Xup0{i}; 44 | 45 | BC{i} = 2*factorFunctions(IC{i},v{i}); 46 | f{i} = IC{i}*a{i} + crf(v{i})*IC{i}*v{i}; 47 | end 48 | 49 | dtau_dq = q{1}(1)*0 + zeros(model.NV,model.NV); 50 | dtau_dqd = q{1}(1)*0 + zeros(model.NV,model.NV); 51 | 52 | J = myCell2Mat(model,S); 53 | Jd = myCell2Mat(model,Sd); 54 | Jdd= myCell2Mat(model,Sdd); 55 | Jj = myCell2Mat(model,Sj); 56 | 57 | tmp1 = repmat(0*q{1}(1) , 6, model.NV); 58 | tmp2 = repmat(0*q{1}(1) , 6, model.NV); 59 | tmp3 = repmat(0*q{1}(1) , 6, model.NV); 60 | tmp4 = repmat(0*q{1}(1) , 6, model.NV); 61 | 62 | dtau_dq = repmat(0*q{1}(1), model.NV,model.NV); 63 | dtau_dqd = repmat(0*q{1}(1), model.NV,model.NV); 64 | 65 | for i = model.NB:-1:1 66 | ii = model.vinds{i}; 67 | 68 | tmp1(:,ii) = IC{i}*S{i}; 69 | tmp2(:,ii) = BC{i}*S{i} + IC{i}*Sj{i}; 70 | tmp3(:,ii) = BC{i}*Sd{i} + IC{i}*Sdd{i} + icrf(f{i})*S{i}; 71 | tmp4(:,ii) = BC{i}.'*S{i}; 72 | 73 | jj = model.subtree_vinds{i}; 74 | 75 | dtau_dq(ii,jj) = J(:,ii).'*tmp3(:,jj); 76 | dtau_dq(jj,ii) = tmp1(:,jj).'*Jdd(:,ii)+tmp4(:,jj).'*Jd(:,ii); 77 | 78 | dtau_dqd(ii,jj) = J(:,ii).'*tmp2(:,jj); 79 | dtau_dqd(jj,ii) = tmp1(:,jj).'*Jj(:,ii)+tmp4(:,jj).'*J(:,ii); 80 | 81 | if model.parent(i) > 0 82 | p = model.parent(i); 83 | IC{p} = IC{p} + IC{i}; 84 | BC{p} = BC{p} + BC{i}; 85 | f{p} = f{p} + f{i}; 86 | end 87 | end 88 | 89 | end 90 | 91 | 92 | function M = myCell2Mat(model,S) 93 | % import casadi.* 94 | % M = SX.zeros(6,model.NV); 95 | % for i = 1:length(S) 96 | % M(:,model.vinds{i}) = S{i}; 97 | % end 98 | M = cell2mat(S); 99 | end 100 | 101 | 102 | -------------------------------------------------------------------------------- /v3/derivatives/StructureConstants.m: -------------------------------------------------------------------------------- 1 | function [s_first_kind, s_second_kind] = StructureConstants(model, q) 2 | 3 | % S_second_kind(i,j,k) = X^i \cdot [X_j , X_k] 4 | % (i.e., the i-th component of the lie bracket between vector fields 5 | % X_j and X_k) 6 | s_second_kind = zeros( model.NV, model.NV, model.NV ); 7 | 8 | % S_first_kind(i,j,k) = H_{i m} S_second_kind(m, j, k) 9 | s_first_kind = zeros( model.NV, model.NV, model.NV ); 10 | 11 | for joint_num = 1:model.NB 12 | joint_inds = model.vinds{joint_num} ; 13 | dofs = length( joint_inds ); 14 | qj = rand( length(model.qinds{joint_num} ) ,1); 15 | 16 | [~, S] = jcalc(model.jtype{joint_num}, qj); 17 | [Q, ~] = qr(S); 18 | Psi = inv([S Q(:,dofs+1:end)])'; 19 | Psi = Psi(:,1:dofs); 20 | 21 | for i = 1:dofs 22 | ii = joint_inds(i); 23 | Si = S(:,i); 24 | for j = 1:dofs 25 | jj = joint_inds(j); 26 | Sj = S(:,j); 27 | s_second_kind(joint_inds, ii,jj ) = Psi'*crm(Si)*Sj; 28 | end 29 | end 30 | end 31 | H = HandC(model,q,zeros(model.NV,1)); 32 | for i = 1:model.NV 33 | s_first_kind(:,:,i) = H*s_second_kind(:,:,i); % index lowering 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /v3/derivatives/modFD_derivatives.m: -------------------------------------------------------------------------------- 1 | function [grad_q, grad_qd, grad_tau] = modFD_derivatives( model, q, qd, tau, lambda ) 2 | 3 | % modFD derivs from modID derivs 4 | 5 | model_no_grav = model; 6 | model_no_grav.gravity = [0;0;0]; 7 | mu = FDab(model_no_grav,q,0*qd, lambda); 8 | 9 | qdd = FDab(model,q,qd,tau); 10 | 11 | [grad_q, grad_qd] = modID_derivatives(model,q,qd, qdd, -mu); 12 | grad_tau = mu; -------------------------------------------------------------------------------- /v3/derivatives/modFD_second_derivatives.m: -------------------------------------------------------------------------------- 1 | function [FD_q, FD_qd, FD_tau, H_qq, H_qdqd, H_qdq, H_tauq] = modFD_second_derivatives( model, q, qd, tau, lambda ) 2 | 3 | % modFD derivs from modID derivs 4 | 5 | model_no_grav = model; 6 | model_no_grav.gravity = [0;0;0]; 7 | 8 | mu = FDab(model_no_grav,q,0*qd, -lambda); 9 | qdd = FDab(model,q,qd,tau); 10 | 11 | derivs = modID_second_derivatives( model, q, qd, qdd, mu); 12 | H_qq = derivs.dmod_dqq; 13 | H_qdqd = derivs.dmod_dvv; 14 | H_qdq = derivs.dmod_dqv'; 15 | dtau_dq = derivs.dtau_dq; 16 | dtau_dqd = derivs.dtau_dv; 17 | 18 | Hinv = Hinverse(model,q); 19 | 20 | % First Order Partials 21 | FD_q = -Hinv*dtau_dq; 22 | FD_qd= -Hinv*dtau_dqd; 23 | FD_tau= Hinv; 24 | 25 | % Second Order Partials 26 | P = modID_derivatives_simple(model, q, mu,[FD_q.' ; FD_qd.' ; FD_tau.'].'); 27 | P_q = P(:,1:model.NV)'; 28 | P_qd = P(:, model.NV+1 : 2*model.NV)'; 29 | P_tau = P(:, 2*model.NV+1 : 3*model.NV)'; 30 | 31 | 32 | H_qq = H_qq + P_q+P_q'; 33 | H_qdq = H_qdq + P_qd; 34 | H_tauq= P_tau; -------------------------------------------------------------------------------- /v3/derivatives/modID_derivatives.m: -------------------------------------------------------------------------------- 1 | function [grad_q, grad_qd, total_grad] = modID_derivatives( model, q, qd, qdd, lambda ) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | 7 | if sum(model.has_rotor) > 1 8 | error('modID does not support rotors'); 9 | end 10 | 11 | a_grav = get_gravity(model); 12 | if ~iscell(q) 13 | [q, qd, qdd, lambda] = confVecToCell(model,q,qd,qdd, lambda); 14 | end 15 | 16 | % Calculate the derivatives using reverse mode chain rule 17 | for i = 1:model.NB 18 | I{i} = q{1}(1)*0+ model.I{i}; 19 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 20 | vJ{i} = S{i}*qd{i}; 21 | wJ{i} = S{i}*lambda{i}; 22 | 23 | Xup{i} = XJ * model.Xtree{i}; 24 | if model.parent(i) == 0 25 | vp{i} = zeros(6,1); 26 | ap{i} = Xup{i}*(-a_grav); 27 | wp{i} = zeros(6,1); 28 | else 29 | vp{i} = Xup{i}*v{model.parent(i)}; 30 | wp{i} = Xup{i}*w{model.parent(i)}; 31 | ap{i} = Xup{i}*a{model.parent(i)}; 32 | end 33 | 34 | Yd{i} = crm(vp{i})*S{i}; 35 | Ydd{i} = crm(vp{i})*Yd{i} + crm(ap{i})*S{i}; 36 | Psid{i} = 2*Yd{i}+crm(vJ{i})*S{i}; 37 | 38 | 39 | v{i} = vp{i} + vJ{i}; 40 | a{i} = ap{i} + crm(v{i})*vJ{i} + S{i}*qdd{i}; 41 | f{i} = I{i}*a{i} + crf(v{i})*I{i}*v{i}; 42 | 43 | w{i} = wp{i} + wJ{i}; 44 | h{i} = I{i}*w{i}; 45 | B{i} = factorFunctions(I{i},v{i}); 46 | z{i} = B{i}.'*w{i}; 47 | end 48 | 49 | grad_q = repmat( 0*q{1}(1), [size(qd,1),size(lambda,2)] ); 50 | grad_qd = repmat(0*0*q{1}(1),[size(qd,1),size(lambda,2)] ); 51 | 52 | for i = model.NB:-1:1 53 | ii = model.vinds{i}; 54 | grad_qd(ii,:) = Psid{i}.'*h{i} + 2*S{i}.'*z{i}; 55 | grad_q(ii,:) = (icrf(f{i})*S{i}).'*wp{i} + 2*Yd{i}.'*z{i} + Ydd{i}.'*h{i}; 56 | p = model.parent(i); 57 | if p > 0 58 | z{p} = z{p}+Xup{i}.'*z{i}; 59 | h{p} = h{p}+Xup{i}.'*h{i}; 60 | f{p} = f{p}+Xup{i}.'*f{i}; 61 | end 62 | end 63 | total_grad = [ grad_q ; grad_qd]; 64 | -------------------------------------------------------------------------------- /v3/derivatives/modID_derivatives_simple.m: -------------------------------------------------------------------------------- 1 | function [grad_q] = modID_derivatives_simple( model, q, qdd, lambda ) 2 | %%% Simplified version of modID_derivatives when gravity off, and qd=0 3 | 4 | if ~isfield(model,'nq') 5 | model = postProcessModel(model); 6 | end 7 | 8 | if sum(model.has_rotor) > 1 9 | error('modID does not support rotors'); 10 | end 11 | 12 | if ~iscell(q) 13 | [q, qdd, lambda] = confVecToCell(model,q,qdd, lambda); 14 | end 15 | 16 | % Calculate the derivatives using reverse mode chain rule 17 | for i = 1:model.NB 18 | I{i} = q{1}(1)*0+ model.I{i}; 19 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 20 | 21 | Xup{i} = XJ * model.Xtree{i}; 22 | if model.parent(i) == 0 23 | X0{i} = Xup{i}; 24 | a{i} = zeros(6,1); 25 | w{i} = zeros(6,1); 26 | else 27 | X0{i} = Xup{i}*X0{model.parent(i)}; 28 | w{i} = w{model.parent(i)}; 29 | a{i} = a{model.parent(i)}; 30 | end 31 | wp{i} = w{i}; 32 | 33 | S{i} = X0{i}\S{i}; 34 | I{i} = X0{i}.'*I{i}*X0{i}; 35 | Ydd{i} = crm(a{i})*S{i}; 36 | 37 | a{i} = a{i} + S{i}*qdd{i}; 38 | w{i} = w{i} + S{i}*lambda{i}; 39 | 40 | h{i} = I{i}*w{i}; 41 | f{i} = I{i}*a{i}; 42 | end 43 | 44 | grad_q = repmat( 0*q{1}(1), [size(q,1),size(lambda,2)] ); 45 | 46 | for i = model.NB:-1:1 47 | ii = model.vinds{i}; 48 | grad_q(ii,:) = Ydd{i}.'*h{i} + (icrf(f{i})*S{i})'*wp{i}; 49 | 50 | p = model.parent(i); 51 | if p > 0 52 | h{p} = h{p}+h{i}; 53 | f{p} = f{p}+f{i}; 54 | end 55 | end 56 | -------------------------------------------------------------------------------- /v3/dynamics/BodyJacobian.m: -------------------------------------------------------------------------------- 1 | function J = BodyJacobian( model, q, body_num, Xend) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) 7 | [q] = confVecToCell(model,q); 8 | end 9 | 10 | J = q{1}(1)*0 + zeros( size(Xend,1), model.NV ); 11 | 12 | X = Xend; 13 | j = body_num; 14 | while j > 0 15 | [ XJ, S ] = jcalc( model.jtype{j}, q{j} ); 16 | jj = model.vinds{j}; 17 | J(:, jj) = X * S; 18 | if model.parent(j) > 0 19 | X = X * XJ * model.Xtree{ j }; 20 | end 21 | j = model.parent(j); 22 | end 23 | -------------------------------------------------------------------------------- /v3/dynamics/BodyJacobianTimeDerivative.m: -------------------------------------------------------------------------------- 1 | function Jdot = BodyJacobianTimeDerivative( model, q, qd, body_num, Xend, Xend_dot) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) 7 | [q,qd] = confVecToCell(model,q,qd); 8 | end 9 | 10 | if nargin == 4 11 | Xend = eye( 6 ); 12 | end 13 | if nargin <= 5 14 | Xend_dot = 0*Xend; 15 | end 16 | 17 | Jdot = q{1}(1)*0 + zeros( size(Xend,1), model.NV ); 18 | 19 | v = {}; 20 | for i = 1:model.NB 21 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 22 | vJ = S{i}*qd{i}; 23 | Xup{i} = XJ * model.Xtree{i}; 24 | if model.parent(i) == 0 25 | v{i} = vJ; 26 | else 27 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 28 | end 29 | Sd{i} = crm(v{i})*S{i}; 30 | end 31 | 32 | X = Xend; 33 | X2 = Xend_dot -Xend * crm( v{body_num} ); 34 | 35 | j = body_num; 36 | while j > 0 37 | jj = model.vinds{j}; 38 | Jdot(:,jj) = X * Sd{j} + X2 * S{j}; 39 | if model.parent(j) > 0 40 | X = X * Xup{j}; 41 | X2 = X2 * Xup{j}; 42 | end 43 | j = model.parent(j); 44 | end -------------------------------------------------------------------------------- /v3/dynamics/CMM.m: -------------------------------------------------------------------------------- 1 | function [A, Ad_qd] = CMM( model, q, qd) 2 | 3 | % Algo from Orin and Goswami (AURO) 4 | assert(~any( model.has_rotor ), 'Rotors not supported for CMM') 5 | 6 | if ~isfield(model,'nq') 7 | model = postProcessModel(model); 8 | end 9 | 10 | if ~iscell(q) 11 | if nargout == 1 12 | [q] = confVecToCell(model,q); 13 | else 14 | [q, qd] = confVecToCell(model, q, qd); 15 | end 16 | end 17 | 18 | 19 | A = q{1}(1)*0 + zeros(6,model.NV); 20 | for i =1:model.NB 21 | IC{i} = q{1}(1)*0 + model.I{i}; 22 | end 23 | I0 = q{1}(1)*0 + zeros(6,6); 24 | 25 | for i = model.NB:-1:1 26 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 27 | Xup{i} = XJ * model.Xtree{i}; 28 | 29 | if model.parent(i) ~= 0 30 | IC{model.parent(i)} = IC{model.parent(i)} + Xup{i}.'*IC{i}*Xup{i}; 31 | else 32 | I0 = I0 + Xup{i}.'*IC{i}*Xup{i}; 33 | end 34 | 35 | end 36 | 37 | M = I0(6,6); %Mass 38 | pG = skew( I0(1:3,4:6)/M ); %CoM position rel. to 0 39 | X0G = [eye(3) zeros(3) ; skew(pG) eye(3)]; 40 | 41 | for i = 1:model.NB 42 | p = model.parent(i); 43 | if p~=0 44 | XiG{i} = Xup{i}*XiG{p}; 45 | else 46 | XiG{i} = Xup{i}*X0G; 47 | end 48 | 49 | ii = model.vinds{i}; 50 | A(:,ii) = XiG{i}.'*IC{i}*S{i}; 51 | end 52 | %info.XiG = XiG; 53 | %info.IC = IC; 54 | 55 | if nargout == 2 56 | model_no_grav = model; 57 | model_no_grav.gravity = [0;0;0]; 58 | [~, info] = ID(model_no_grav, cell2mat(q), cell2mat(qd), 0*cell2mat(qd)); 59 | Ad_qd = X0G.'*info.f0; 60 | end 61 | 62 | -------------------------------------------------------------------------------- /v3/dynamics/CMMTimeDerivative.m: -------------------------------------------------------------------------------- 1 | function [Adot, info] = CMMTimeDerivative( model, q,qd ) 2 | 3 | % Algo from Orin and Goswami (AURO) 4 | 5 | assert(~any( model.has_rotor ), 'Rotors not supported for CMM') 6 | %assert(strcmp(model.jtype(1),'Fb'), 'First joint should be floating base'); 7 | 8 | 9 | if ~isfield(model,'nq') 10 | model = postProcessModel(model); 11 | end 12 | 13 | if ~iscell(q) 14 | [q,qd] = confVecToCell(model,q,qd); 15 | end 16 | 17 | Adot = q{1}(1)*0 + zeros( 6, model.NV ); 18 | 19 | v = {}; 20 | for i = 1:model.NB 21 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 22 | vJ = S{i}*qd{i}; 23 | Xup{i} = XJ * model.Xtree{i}; 24 | if model.parent(i) == 0 25 | v{i} = vJ; 26 | else 27 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 28 | end 29 | Sd{i} = crm(v{i})*S{i}; 30 | Id{i} = crf(v{i})*model.I{i}-model.I{i}*crm( v{i} ); % Time derivative of inertia. Derivative taken in world frame, then expressed in local. 31 | end 32 | 33 | 34 | A = q{1}(1)*0 + zeros(6,model.NV); 35 | for i =1:model.NB 36 | IC{i} = q{1}(1)*0 + model.I{i}; 37 | ICd{i} = q{1}(1)*0 + Id{i}; % Time derivative of composite inertia. Derivative taken in world frame, then expressed in local. 38 | end 39 | I0 = q{1}(1)*0 + zeros(6,6); 40 | I0d = q{1}(1)*0 + zeros(6,6); 41 | 42 | for i = model.NB:-1:1 43 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 44 | Xup{i} = XJ * model.Xtree{i}; 45 | 46 | if model.parent(i) ~= 0 47 | IC{model.parent(i)} = IC{model.parent(i)} + Xup{i}.'*IC{i}*Xup{i}; 48 | ICd{model.parent(i)} = ICd{model.parent(i)} + Xup{i}.'*ICd{i}*Xup{i}; 49 | else 50 | I0 = I0 + Xup{i}.'*IC{i}*Xup{i}; 51 | I0d = I0d + Xup{i}.'*ICd{i}*Xup{i}; 52 | end 53 | end 54 | 55 | M = I0(6,6); %Mass 56 | pG = skew( I0(1:3,4:6)/M ); %CoM position rel. to 0 57 | X0G = [eye(3) zeros(3) ; skew(pG) eye(3)]; 58 | vG = skew( I0d(1:3,4:6)/M ); 59 | vG = [zeros(3,1) ; vG]; 60 | 61 | 62 | for i = 1:model.NB 63 | p = model.parent(i); 64 | if p~=0 65 | XiG{i} = Xup{i}*XiG{p}; 66 | else 67 | XiG{i} = Xup{i}*X0G; 68 | end 69 | 70 | ii = model.vinds{i}; 71 | A(:,ii) = XiG{i}.'*IC{i}*S{i}; 72 | Adot(:,ii) = XiG{i}.'*(ICd{i}*S{i}+IC{i}*Sd{i}); 73 | end 74 | 75 | % At this point, Adot has time derivative taken in world frame. Need to add 76 | % correction for derivative taken in moving G frame. 77 | Adot = Adot - crf(vG)*A; -------------------------------------------------------------------------------- /v3/dynamics/CMM_from_CRBA.m: -------------------------------------------------------------------------------- 1 | function [A] = CMM_from_CRBA( model, q) 2 | 3 | % Algo from Wensing and Orin (IJHR) 4 | 5 | assert(strcmp(model.jtype(1),'Fb'), 'First joint should be floating base'); 6 | 7 | if ~isfield(model,'nq') 8 | model = postProcessModel(model); 9 | end 10 | qd = q(1)*0 + zeros(model.NV,1); 11 | if ~iscell(q) 12 | [q,qd] = confVecToCell(model,q,qd); 13 | end 14 | 15 | 16 | 17 | % Compute CMM from Mass Matrix 18 | [H] = HandC(model,q,qd); 19 | 20 | % Joint model for floating base 21 | [X10, Phi ] = jcalc( model.jtype{1}, q{1} ); 22 | 23 | Psi = inv(Phi); 24 | H11 = H(1:6,1:6); 25 | 26 | IC = Psi'* H11 * Psi; 27 | 28 | [~, p1G] = mcI( IC ); % extract CoM position rel to FB 29 | 30 | R1G = X10(1:3,1:3); 31 | X1G = [R1G zeros(3); skew(p1G)*R1G R1G]; 32 | 33 | A = X1G' * Psi' * H(1:6, :); -------------------------------------------------------------------------------- /v3/dynamics/Christoffel.m: -------------------------------------------------------------------------------- 1 | function [Gamma, out] = Christoffel(model,q) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) || ~iscell(qd) 7 | [q] = confVecToCell(model,q); 8 | end 9 | if sum(model.has_rotor) > 1 10 | error('Christoffel does not support rotors (yet)'); 11 | end 12 | 13 | for i = 1:model.NB 14 | IC{i} = q{1}(1)*0 + model.I{i}; 15 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 16 | Xup{i} = XJ * model.Xtree{i}; 17 | if model.parent(i) == 0 18 | Xup0{i} = Xup{i}; 19 | else 20 | Xup0{i} = Xup{i}*Xup0{ model.parent(i) }; 21 | end 22 | 23 | S{i} = Xup0{i}\S{i}; 24 | IC{i} = Xup0{i}.'*IC{i}*Xup0{i}; 25 | end 26 | 27 | for k = model.NB:-1:1 28 | for k_ind = 1:length( model.vinds{k} ) 29 | kk = model.vinds{k}(k_ind); 30 | sk = S{k}(:,k_ind); 31 | B = factorFunctions(IC{k},sk); 32 | 33 | j = k; 34 | while j > 0 35 | jj = model.vinds{j}; 36 | f1 = B * S{j}; 37 | f2 = B.'* S{j}; 38 | i=j; 39 | while i > 0 40 | ii = model.vinds{i}; 41 | 42 | Gamma(ii,jj,kk) = S{i}.'*f1; 43 | if j < k 44 | Gamma(ii,kk,jj) = S{i}.'*f1; 45 | end 46 | 47 | Gamma(jj,ii,kk) = ( S{i}.'*f2 ).'; 48 | if i < k 49 | Gamma(jj,kk,ii) = ( S{i}.'*f2 ).'; 50 | end 51 | 52 | Gamma(kk,ii,jj) = -S{i}.'*f2; 53 | if i < j 54 | Gamma(kk,jj,ii) = ( -S{i}.'*f2).'; 55 | end 56 | 57 | i = model.parent(i); 58 | end 59 | j = model.parent(j); 60 | end 61 | end 62 | 63 | if model.parent(k) > 0 64 | p = model.parent(k); 65 | IC{p}= IC{p} + IC{k}; 66 | end 67 | end 68 | 69 | out.S = S; 70 | out.IC = IC; -------------------------------------------------------------------------------- /v3/dynamics/EnerMo.m: -------------------------------------------------------------------------------- 1 | function ret = EnerMo( model, q, qd ) 2 | 3 | % EnerMo calculate energy, momentum and related quantities 4 | % EnerMo(robot,q,qd) returns a structure containing the fields KE, PE, 5 | % htot, Itot, mass, cm and vcm. These fields contain the kinetic and 6 | % potential energies of the whole system, the total spatial momentum, the 7 | % total spatial inertia, total mass, position of centre of mass, and the 8 | % linear velocity of centre of mass, respectively. Vector quantities are 9 | % expressed in base coordinates. PE is defined to be zero when cm is 10 | % zero. 11 | 12 | if ~isfield(model,'nq') 13 | model = postProcessModel(model); 14 | end 15 | 16 | [q, qd] = confVecToCell(model,q,qd); 17 | 18 | % if sum(model.has_rotor) > 1 19 | % error('EnerMo does not support rotors'); 20 | % end 21 | 22 | 23 | for i = 1:model.NB 24 | [ XJ, S ] = jcalc( model.jtype{i}, q{i} ); 25 | vJ = S*qd{i}; 26 | Xup{i} = XJ * model.Xtree{i}; 27 | if model.parent(i) == 0 28 | v{i} = vJ; 29 | else 30 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 31 | end 32 | 33 | Ic{i} = model.I{i}; 34 | hc{i} = Ic{i} * v{i}; 35 | KE(i) = 0.5 * v{i}.' * hc{i}; 36 | if model.has_rotor(i) 37 | [ XJ_rotor, S_rotor{i} ] = jcalc( model.jtype_rotor{i}, q{i}*model.gr{i} ); 38 | S_rotor{i} = S_rotor{i} * model.gr{i}; 39 | vJ_rotor = S_rotor{i} * qd{i}; 40 | Xup_rotor{i} = XJ_rotor * model.Xrotor{i}; 41 | if model.parent(i) == 0 42 | v_rotor{i} = vJ_rotor; 43 | else 44 | v_rotor{i} = Xup_rotor{i}*v{model.parent(i)} + vJ_rotor; 45 | end 46 | h_rotor{i} = model.I_rotor{i}*v_rotor{i}; 47 | KE(i) = KE(i) + 1/2*h_rotor{i}.'*v_rotor{i}; 48 | end 49 | end 50 | 51 | ret.Itot = q{1}(1)*0 + zeros(size(Ic{1})); 52 | ret.htot = q{1}(1)*0 + zeros(size(hc{1})); 53 | 54 | for i = model.NB:-1:1 55 | if model.parent(i) ~= 0 56 | Ic{model.parent(i)} = Ic{model.parent(i)} + Xup{i}.'*Ic{i}*Xup{i}; 57 | hc{model.parent(i)} = hc{model.parent(i)} + Xup{i}.'*hc{i}; 58 | 59 | if model.has_rotor(i) % Modified backward pass 60 | Ic{model.parent(i)} = Ic{model.parent(i)} + Xup_rotor{i}.'*model.I_rotor{i}*Xup_rotor{i}; 61 | hc{model.parent(i)} = hc{model.parent(i)} + Xup_rotor{i}.'* h_rotor{i}; 62 | end 63 | else 64 | ret.Itot = ret.Itot + Xup{i}.'*Ic{i}*Xup{i}; 65 | ret.htot = ret.htot + Xup{i}.'*hc{i}; 66 | if model.has_rotor(i) % Modified backward pass 67 | ret.Itot = ret.Itot + Xup_rotor{i}.'*model.I_rotor{i}*Xup_rotor{i}; 68 | ret.htot = ret.htot + Xup_rotor{i}.'* h_rotor{i}; 69 | end 70 | end 71 | end 72 | 73 | a_grav = get_gravity(model); 74 | 75 | if length(a_grav) == 6 76 | g = a_grav(4:6); % 3D linear gravitational accn 77 | h = ret.htot(4:6); % 3D linear momentum 78 | else 79 | g = a_grav(2:3); % 2D gravity 80 | h = ret.htot(2:3); % 2D linear momentum 81 | end 82 | 83 | [mass, cm] = mcI(ret.Itot); 84 | 85 | ret.KE = sum(KE); 86 | ret.PE = - mass * dot(cm,g); 87 | ret.mass = mass; 88 | ret.cm = cm; 89 | ret.vcm = h / mass; 90 | -------------------------------------------------------------------------------- /v3/dynamics/HamiltonianDynamics.m: -------------------------------------------------------------------------------- 1 | function [q_dot, p_dot] = HamiltonianDynamics( model, q , p, tau, f_ext) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | 7 | model_no_grav = model; 8 | model_no_grav.gravity = [0 0 0]'; 9 | qd = FDab( model_no_grav , q, zeros(model.NV,1), p); 10 | if nargin == 5 11 | tau_g = ID(model, q, zeros(model.NV,1), zeros(model.NV,1), f_ext); 12 | else 13 | tau_g = ID(model, q, zeros(model.NV,1), zeros(model.NV,1)); 14 | end 15 | 16 | if ~iscell(q) 17 | [q, qd, p] = confVecToCell(model,q,qd,p); 18 | end 19 | 20 | 21 | 22 | for i = 1:model.NB 23 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 24 | vJ = S{i}*qd{i}; 25 | Xup{i} = XJ * model.Xtree{i}; 26 | if model.parent(i) == 0 27 | v{i} = vJ; 28 | else 29 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 30 | end 31 | h{i} = model.I{i}*v{i}; 32 | Sd{i} = crm(v{i})*S{i}; 33 | 34 | % Extra data for rotors 35 | if model.has_rotor(i) 36 | [ XJ_rotor, S_rotor{i} ] = jcalc( model.jtype_rotor{i}, q{i}*model.gr{i} ); 37 | S_rotor{i} = S_rotor{i} * model.gr{i}; 38 | vJ_rotor = S_rotor{i} * qd{i}; 39 | Xup_rotor{i} = XJ_rotor * model.Xrotor{i}; 40 | if model.parent(i) == 0 41 | v_rotor{i} = vJ_rotor; 42 | else 43 | v_rotor{i} = Xup_rotor{i}*v{model.parent(i)} + vJ_rotor; 44 | end 45 | h_rotor{i} = model.I_rotor{i}*v_rotor{i}; 46 | Sd_rotor{i} = crm(v_rotor{i})*S_rotor{i}; 47 | end 48 | end 49 | 50 | CTqd= q{1}(1)*0 + zeros(model.NV,1); 51 | q_dot = q{1}(1)*0 + zeros(model.NV,1); 52 | 53 | for i = model.NB:-1:1 54 | ii = model.vinds{i}; 55 | q_dot(ii) = qd{i}; 56 | CTqd(ii)= Sd{i}'* h{i}; 57 | if model.parent(i) ~= 0 58 | p = model.parent(i); 59 | h{p} = h{p} + Xup{i}'*h{i}; 60 | end 61 | if model.has_rotor(i) 62 | CTqd(ii)= CTqd(ii)+ Sd_rotor{i}'* h_rotor{i}; 63 | if model.parent(i) ~= 0 64 | p = model.parent(i); 65 | h{p} = h{p} + Xup_rotor{i}'*h_rotor{i}; 66 | end 67 | end 68 | end 69 | 70 | p_dot = CTqd + tau - tau_g; 71 | -------------------------------------------------------------------------------- /v3/dynamics/Hinverse.m: -------------------------------------------------------------------------------- 1 | function [Hinv] = Hinverse( model, q) 2 | % 3 | % Factored ABA algorithm for computing the inverse of the mass matrix H 4 | % 5 | % Output Hinv satisfies: Hinv = inv(H) 6 | % 7 | % This condition is equivalent to: 8 | % q_ddot = Hinv * tau when q_dot=0 and a_grav = 0 9 | % 10 | 11 | if ~isfield(model,'nq') 12 | model = postProcessModel(model); 13 | end 14 | if ~iscell(q) 15 | [q] = confVecToCell(model,q); 16 | end 17 | if sum(model.has_rotor) > 1 18 | error('Hinverse does not support rotors'); 19 | end 20 | 21 | 22 | IA = model.I; % Cell array for Artiulated-Body Inertias 23 | F = repmat({q{1}(1)*0 + zeros(6,model.NV)},model.NV,1); % Satisfies F{i}*tau = pA{i} from usual ABA 24 | P = repmat({q{1}(1)*0 + zeros(6,model.NV)},model.NV,1); % Satisfies P{i}*tau = a{i} from usual ABA 25 | 26 | Hinv = q{1}(1)*0 + zeros(model.NV); % Inerse of Mass Matrix 27 | 28 | % Outward Pass 29 | for i = 1:model.NB 30 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 31 | Xup{i} = XJ * model.Xtree{i}; 32 | end 33 | 34 | % Inward Pass 35 | for i = model.NB:-1:1 36 | ii = model.vinds{i}; 37 | U{i} = IA{i}*S{i}; 38 | D{i} = S{i}.'*U{i}; 39 | Hinv(ii,:) = -D{i}\S{i}.'*F{i}; 40 | Hinv(ii,ii) = Hinv(i,i) + inv(D{i}); 41 | % Note: At this point, Hinv(i,:) is not equal to its final value. 42 | % However, it does satisfy Hinv(i,:)*tau = D{i}\u{i} from usual ABA 43 | 44 | p = model.parent(i); 45 | if p > 0 46 | Fa = F{i} + U{i}*Hinv(ii,:); % Bias force transmitted to predecessor 47 | Ia = IA{i} - U{i}*(D{i}\U{i}.'); % Articulated inertia transmitted to predeceesor 48 | 49 | F{p} = F{p} + Xup{i}.'*Fa; 50 | IA{p} = IA{p} + Xup{i}.'*Ia*Xup{i}; 51 | end 52 | end 53 | 54 | % Outward Pass 55 | for i = 1:model.NB 56 | p = model.parent(i); 57 | ii = model.vinds{i}; 58 | if p > 0 59 | Hinv(ii,:) = Hinv(ii,:) - D{i}\U{i}.'*Xup{i}*P{p}; 60 | P{i} = Xup{i}*P{p} + S{i}*Hinv(ii,:); 61 | else 62 | P{i} = S{i}*Hinv(ii,:); 63 | end 64 | end -------------------------------------------------------------------------------- /v3/dynamics/Hqd_and_CTqd.m: -------------------------------------------------------------------------------- 1 | function [Hqd, CTqd] = Hqd_and_CTqd( model, q , qd) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) 7 | [q, qd] = confVecToCell(model,q,qd); 8 | end 9 | 10 | 11 | for i = 1:model.NB 12 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 13 | vJ = S{i}*qd{i}; 14 | Xup{i} = XJ * model.Xtree{i}; 15 | if model.parent(i) == 0 16 | v{i} = vJ; 17 | else 18 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 19 | end 20 | h{i} = model.I{i}*v{i}; 21 | Sd{i} = crm(v{i})*S{i}; 22 | 23 | 24 | % Extra data for rotors 25 | if model.has_rotor(i) 26 | [ XJ_rotor, S_rotor{i} ] = jcalc( model.jtype_rotor{i}, q{i}*model.gr{i} ); 27 | S_rotor{i} = S_rotor{i} * model.gr{i}; 28 | vJ_rotor = S_rotor{i} * qd{i}; 29 | Xup_rotor{i} = XJ_rotor * model.Xrotor{i}; 30 | if model.parent(i) == 0 31 | v_rotor{i} = vJ_rotor; 32 | else 33 | v_rotor{i} = Xup_rotor{i}*v{model.parent(i)} + vJ_rotor; 34 | end 35 | h_rotor{i} = model.I_rotor{i}*v_rotor{i}; 36 | Sd_rotor{i} = crm(v_rotor{i})*S_rotor{i}; 37 | end 38 | 39 | end 40 | 41 | Hqd = q{1}(1)*0 + zeros(model.NV,1); 42 | CTqd= q{1}(1)*0 + zeros(model.NV,1); 43 | 44 | for i = model.NB:-1:1 45 | ii = model.vinds{i}; 46 | Hqd(ii) = S{i}' * h{i}; 47 | CTqd(ii)= Sd{i}'* h{i}; 48 | if model.parent(i) ~= 0 49 | p = model.parent(i); 50 | h{p} = h{p} + Xup{i}'*h{i}; 51 | end 52 | 53 | if model.has_rotor(i) 54 | Hqd(ii) = Hqd(ii) + S_rotor{i}' * h_rotor{i}; 55 | CTqd(ii)= CTqd(ii)+ Sd_rotor{i}'* h_rotor{i}; 56 | if model.parent(i) ~= 0 57 | p = model.parent(i); 58 | h{p} = h{p} + Xup_rotor{i}'*h_rotor{i}; 59 | end 60 | end 61 | end 62 | 63 | -------------------------------------------------------------------------------- /v3/dynamics/ID.m: -------------------------------------------------------------------------------- 1 | function [tau, out] = ID( model, q, qd, qdd, f_ext ) 2 | 3 | % ID Inverse Dynamics via Recursive Newton-Euler Algorithm 4 | % ID(model,q,qd,qdd,f_ext) calculates the inverse dynamics of a kinematic 5 | % tree via the recursive Newton-Euler algorithm. q, qd and qdd are vectors 6 | % of joint position, velocity and acceleration variables; and the return 7 | % value is a vector of joint force variables. f_ext is an optional 8 | % argument specifying the external forces acting on the bodies. It can be 9 | % omitted if there are no external forces. The format of f_ext is 10 | % explained in the source code of apply_external_forces. 11 | 12 | if ~isfield(model,'nq') 13 | model = postProcessModel(model); 14 | end 15 | 16 | a_grav = get_gravity(model); 17 | if ~iscell(q) 18 | [q, qd, qdd] = confVecToCell(model,q,qd,qdd); 19 | end 20 | 21 | 22 | for i = 1:model.NB 23 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 24 | vJ = S{i}*qd{i}; 25 | Xup{i} = XJ * model.Xtree{i}; 26 | if model.parent(i) == 0 27 | v{i} = vJ; 28 | a{i} = Xup{i}*(-a_grav) + S{i}*qdd{i}; 29 | else 30 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 31 | a{i} = Xup{i}*a{model.parent(i)} + S{i}*qdd{i} + crm(v{i})*vJ; 32 | end 33 | h{i} = model.I{i}*v{i}; 34 | f{i} = model.I{i}*a{i} + crf(v{i})*h{i}; 35 | 36 | % Extra data for rotors 37 | if model.has_rotor(i) 38 | [ XJ_rotor, S_rotor{i} ] = jcalc( model.jtype_rotor{i}, q{i}*model.gr{i} ); 39 | S_rotor{i} = S_rotor{i} * model.gr{i}; 40 | vJ_rotor = S_rotor{i} * qd{i}; 41 | Xup_rotor{i} = XJ_rotor * model.Xrotor{i}; 42 | if model.parent(i) == 0 43 | v_rotor{i} = vJ_rotor; 44 | a_rotor{i} = Xup_rotor{i}*(-a_grav) + S_rotor{i}*qdd{i}; 45 | else 46 | v_rotor{i} = Xup_rotor{i}*v{model.parent(i)} + vJ_rotor; 47 | a_rotor{i} = Xup_rotor{i}*a{model.parent(i)} + S_rotor{i}*qdd{i} + crm(v_rotor{i})*vJ_rotor; 48 | end 49 | f_rotor{i} = model.I_rotor{i}*a_rotor{i} + crf(v_rotor{i})*model.I_rotor{i}*v_rotor{i}; 50 | end 51 | end 52 | 53 | out.f0 = f; 54 | 55 | if nargin == 5 56 | f = apply_external_forces( model.parent, Xup, f, f_ext ); 57 | end 58 | 59 | % This line ensures that ID is symbolic or Casadi compatible 60 | tau = q{1}(1)*0 + zeros(model.NV,1); 61 | f0 = zeros(6,1); 62 | 63 | for i = model.NB:-1:1 64 | p = model.parent(i); 65 | 66 | ii = model.vinds{i}; 67 | tau(ii) = S{i}.' * f{i}; 68 | if p ~= 0 69 | f{p} = f{p} + Xup{i}.'*f{i} ; 70 | else 71 | f0 = f0+Xup{i}.'*f{i} ; 72 | end 73 | 74 | if model.has_rotor(i) % Modified backward pass 75 | tau(ii) = tau(ii) + S_rotor{i}' * f_rotor{i}; 76 | if p ~= 0 77 | f{p} = f{p} + Xup_rotor{i}.'*f_rotor{i}; 78 | else 79 | f0 = f0+Xup_rotor{i}.'*f_rotor{i} ; 80 | end 81 | end 82 | end 83 | 84 | out.Xup = Xup; 85 | out.v = v; 86 | out.h = h; 87 | out.a = a; 88 | out.f = f; 89 | out.tau = tau; 90 | out.f0 = f0; 91 | -------------------------------------------------------------------------------- /v3/dynamics/ID_SlotineLi.m: -------------------------------------------------------------------------------- 1 | function [tau,info] = ID_SlotineLI( model, q, qd , qd_r, qdd, factorFunction) 2 | 3 | if nargin == 5 4 | factorFunction = @(I,v)(factorFunctions(I,v)); 5 | end 6 | 7 | if ~isfield(model,'nq') 8 | model = postProcessModel(model); 9 | end 10 | if ~iscell(q) 11 | [q,qd,qd_r,qdd] = confVecToCell(model,q,qd,qd_r,qdd); 12 | 13 | end 14 | 15 | a_grav = get_gravity(model); 16 | p = model.parent; 17 | 18 | for i = 1:model.NB 19 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 20 | vJ = S{i}*qd{i}; 21 | wJ = S{i}*qd_r{i}; 22 | Xup{i} = XJ * model.Xtree{i}; 23 | if p(i) == 0 24 | v{i} = vJ; 25 | w{i} = wJ; 26 | wd{i}= Xup{i}*(-a_grav) + S{i} * qdd{i} + crm(v{i})*wJ; 27 | else 28 | v{i} = Xup{i}*v{p(i)} + vJ; 29 | w{i} = Xup{i}*w{p(i)} + wJ; 30 | wd{i}= Xup{i}*wd{p(i)} + S{i} * qdd{i} + crm(v{i})*wJ; 31 | end 32 | f{i} = model.I{i}*wd{i} + factorFunction(model.I{i}, v{i})*w{i}; 33 | 34 | % Extra data for rotors 35 | if model.has_rotor(i) 36 | [ XJ_rotor, S_rotor{i} ] = jcalc( model.jtype_rotor{i}, q{i}*model.gr{i} ); 37 | S_rotor{i} = S_rotor{i} * model.gr{i}; 38 | vJ_rotor = S_rotor{i} * qd{i}; 39 | wJ_rotor = S_rotor{i} * qd_r{i}; 40 | 41 | Xup_rotor{i} = XJ_rotor * model.Xrotor{i}; 42 | if p(i) == 0 43 | v_rotor{i} = vJ_rotor; 44 | w_rotor{i} = wJ_rotor; 45 | wd_rotor{i} = Xup_rotor{i}*(-a_grav) + S_rotor{i}*qdd{i}; 46 | else 47 | v_rotor{i} = Xup_rotor{i}*v{p(i)} + vJ_rotor; 48 | w_rotor{i} = Xup_rotor{i}*w{p(i)} + wJ_rotor; 49 | wd_rotor{i} = Xup_rotor{i}*wd{p(i)} + S_rotor{i}*qdd{i} + crm(v_rotor{i})*wJ_rotor; 50 | end 51 | f_rotor{i} = model.I_rotor{i}*wd_rotor{i} + factorFunction(model.I_rotor{i}, v_rotor{i})*w_rotor{i}; 52 | end 53 | 54 | end 55 | 56 | tau = q{1}(1)*0 + zeros(model.NV,1); 57 | for i = model.NB:-1:1 58 | ii = model.vinds{i}; 59 | tau(ii) = S{i}' * f{i} ; 60 | 61 | if p(i) ~= 0 62 | f{p(i)} = f{p(i)} + Xup{i}'*f{i} ; 63 | end 64 | 65 | if model.has_rotor(i) % Modified backward pass 66 | tau(ii) = tau(ii) + S_rotor{i}' * f_rotor{i}; 67 | if p(i) ~= 0 68 | f{p(i)} = f{p(i)} + Xup_rotor{i}'*f_rotor{i}; 69 | end 70 | end 71 | end 72 | -------------------------------------------------------------------------------- /v3/dynamics/jcalc.m: -------------------------------------------------------------------------------- 1 | function [Xj,S] = jcalc( jtyp, q ) 2 | 3 | % jcalc joint transform and motion subspace matrices. 4 | % [Xj,S]=jcalc(type,q) returns the joint transform and motion subspace 5 | % matrices for a joint of the given type. jtyp can be either a string or a 6 | % structure containing a string-valued field called 'code'. Either way, 7 | % the string contains the joint type code. For joints that take 8 | % parameters (e.g. helical), jtyp must be a structure, and it must contain 9 | % a field called 'pars', which in turn is a structure containing one or 10 | % more parameters. (For a helical joint, pars contains a parameter called 11 | % 'pitch'.) q is the joint's position variable. 12 | 13 | if ischar( jtyp ) 14 | code = jtyp; 15 | else 16 | code = jtyp.code; 17 | end 18 | 19 | switch code 20 | case 'Rx' % revolute X axis 21 | Xj = rotx(q); 22 | S = [1;0;0;0;0;0]; 23 | case 'Ry' % revolute Y axis 24 | Xj = roty(q); 25 | S = [0;1;0;0;0;0]; 26 | case {'R','Rz'} % revolute Z axis 27 | Xj = rotz(q); 28 | S = [0;0;1;0;0;0]; 29 | case 'Px' % prismatic X axis 30 | Xj = xlt([q 0 0]); 31 | S = [0;0;0;1;0;0]; 32 | case 'Py' % prismatic Y axis 33 | Xj = xlt([0 q 0]); 34 | S = [0;0;0;0;1;0]; 35 | case {'P','Pz'} % prismatic Z axis 36 | Xj = xlt([0 0 q]); 37 | S = [0;0;0;0;0;1]; 38 | case {'S'} % spherical via quaternion 39 | Xj = plux( rq(q), [0 0 0]); 40 | S = [eye(3);zeros(3)]; 41 | case 'SO3' % spherical via rotation matrix 42 | Xj = plux(reshape(q,[3 3]), [0 0 0]); 43 | S = [eye(3); zeros(3)]; 44 | case 'H' % helical (Z axis) 45 | Xj = rotz(q) * xlt([0 0 q*jtyp.pars.pitch]); 46 | S = [0;0;1;0;0;jtyp.pars.pitch]; 47 | case 'r' % planar revolute 48 | Xj = plnr( q, [0 0] ); 49 | S = [1;0;0]; 50 | case 'px' % planar prismatic X axis 51 | Xj = plnr( 0, [q 0] ); 52 | S = [0;1;0]; 53 | case 'py' % planar prismatic Y axis 54 | Xj = plnr( 0, [0 q] ); 55 | S = [0;0;1]; 56 | case {'Rx-', 'Ry-','Rz-','Px-','Py-','Pz-','r-','px-','py-'} 57 | [Xj, S] = jcalc( jtyp(1:end-1), -q ); 58 | S = -S; 59 | case 'Fb' 60 | S = eye(6); 61 | Xj = plux( rq(q(1:4)), q(5:7) ); 62 | case 'SE3' 63 | S = eye(6); 64 | T = reshape(q,[4 4]); 65 | R = T(1:3,1:3); 66 | p = T(1:3,4); 67 | Xj = [R zeros(3); skew(p)*R R]; 68 | otherwise 69 | error( 'unrecognised joint code ''%s''', code ); 70 | end 71 | -------------------------------------------------------------------------------- /v3/dynamics/jinfo.m: -------------------------------------------------------------------------------- 1 | function [nq,nv] = jinfo( jtyp) 2 | 3 | if ischar( jtyp ) 4 | code = jtyp; 5 | else 6 | code = jtyp.code; 7 | end 8 | 9 | switch code 10 | case {'Rx','Ry','Rz','R','Px','Py','Pz','P','px','py','r','H','Rx-','Ry-','Rz-','Px-','Py-','Pz-','px-','py-','r-'} 11 | nq = 1; 12 | nv = 1; 13 | case {'S'} % spherical 14 | nq = 4; 15 | nv = 3; 16 | case 'Fb' 17 | nq = 7; 18 | nv = 6; 19 | case 'SO3' 20 | nq = 9; 21 | nv = 3; 22 | case 'SE3' 23 | nq = 16; 24 | nv = 6; 25 | otherwise 26 | error( 'unrecognised joint code ''%s''', code ); 27 | end 28 | -------------------------------------------------------------------------------- /v3/dynamics/modFD.m: -------------------------------------------------------------------------------- 1 | function out = modFD( model, q, qd, tau, mu ) 2 | 3 | qdd = FDab(model,q,qd,tau); 4 | out = mu.'*qdd; 5 | -------------------------------------------------------------------------------- /v3/dynamics/modID.m: -------------------------------------------------------------------------------- 1 | function out = modID( model, q, qd, qdd, lambda ) 2 | 3 | % ID Inverse Dynamics via Recursive Newton-Euler Algorithm 4 | % ID(model,q,qd,qdd,f_ext) calculates the inverse dynamics of a kinematic 5 | % tree via the recursive Newton-Euler algorithm. q, qd and qdd are vectors 6 | % of joint position, velocity and acceleration variables; and the return 7 | % value is a vector of joint force variables. f_ext is an optional 8 | % argument specifying the external forces acting on the bodies. It can be 9 | % omitted if there are no external forces. The format of f_ext is 10 | % explained in the source code of apply_external_forces. 11 | 12 | if ~isfield(model,'nq') 13 | model = postProcessModel(model); 14 | end 15 | 16 | if sum(model.has_rotor) > 1 17 | error('modID does not support rotors'); 18 | end 19 | 20 | a_grav = get_gravity(model); 21 | if ~iscell(q) 22 | [q, qd, qdd, lambda] = confVecToCell(model,q,qd,qdd, lambda); 23 | end 24 | 25 | out=q{1}(1)*0 + 0; 26 | for i = 1:model.NB 27 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 28 | vJ = S{i}*qd{i}; 29 | wJ = S{i}*lambda{i}; 30 | 31 | Xup{i} = XJ * model.Xtree{i}; 32 | if model.parent(i) == 0 33 | v{i} = vJ; 34 | w{i} = wJ; 35 | a{i} = Xup{i}*(-a_grav) + S{i}*qdd{i}; 36 | else 37 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 38 | w{i} = Xup{i}*w{model.parent(i)} + wJ; 39 | a{i} = Xup{i}*a{model.parent(i)} + S{i}*qdd{i} + crm(v{i})*vJ; 40 | end 41 | out = out + w{i}.'*(model.I{i}*a{i} + crf(v{i})*model.I{i}*v{i}); 42 | 43 | end -------------------------------------------------------------------------------- /v3/identifiability/Algorithms/RPNA.m: -------------------------------------------------------------------------------- 1 | function [N, M, V , C] = RPNA(model, free_base) 2 | %% [N, M, V , C] = RPNA(model, free_base) 3 | % Recursive parameter nullspace algorithm 4 | % The outputs of the algorithm are 5 | % N: cell array for the nullspace descriptors characterizing undetectable 6 | % inertial transfers across each joint 7 | % M: Basis for minimal paramers (sometimes called representative base 8 | % parameters) of each link 9 | % V: Attainable velocity span basis for each link 10 | % C: Null(C) gives the unidentifiable parameters of each link 11 | % 12 | % See the accompanying paper for further detail. 13 | % 14 | if nargin == 1 15 | free_base = 0; 16 | end 17 | 18 | % Initialize Empty Cell arrays 19 | EmptyCells = cell(model.NB,1); 20 | N = EmptyCells; C = EmptyCells; O = EmptyCells; 21 | M = EmptyCells; V = EmptyCells; 22 | V_J = EmptyCells; 23 | 24 | % Main algorithm loop 25 | for i =1:model.NB 26 | [~, Si] = jcalc( model.jtype{i}, 0 ); 27 | n_i = size(Si, 2); 28 | 29 | p = model.parent(i); 30 | if p == 0 31 | % If the base is to be treated as free, set the motion and 32 | % outer product bases to represent the full space. 33 | % Else initialize them from the fixed base. 34 | if free_base 35 | Vp = eye(6); 36 | Cp = eye(10); 37 | else 38 | Vp = get_gravity(model); 39 | Cp = zeros(0,10); 40 | end 41 | else 42 | Vp = V{p}; 43 | Cp = C{p}; 44 | end 45 | 46 | % Propage the velocity and outer product spans across the link 47 | V_J{i} = model.Xtree{i} * Vp; 48 | 49 | % Compute the collection of rate matricies for multi-DoF joints 50 | crmSet = cell( n_i, 1); 51 | paramRateSet = cell( n_i, 1); 52 | for k= 1:size(Si,2) 53 | % Velocity rates of change with joint angle 54 | crmSet{k} = crm(Si(:,k) ); 55 | 56 | % Inertail parameters rates of change 57 | paramRateSet{k} = Rate_Parameters( Si(:,k) ); 58 | end 59 | 60 | % Propagate the velocity across a joint 61 | V{i} = RangeBasis([ SwitchedControllabilityMatrix( crmSet , V_J{i} ) Si]); 62 | 63 | O{i} = SwitchedObservabilityMatrix(Cp*Transform_Parameters( model.Xtree{i} ), paramRateSet ); 64 | N{i} = OutputMatrix(V{i},Si); 65 | for k = 1:n_i 66 | N{i} = [N{i} ; O{i} * paramRateSet{k}]; 67 | end 68 | N{i} = RangeBasis(N{i}')'; 69 | C{i} = RangeBasis( [ Cp*Transform_Parameters( model.Xtree{i} ) ; N{i} ]')'; 70 | M{i} = UnitVectorComplementarySubspace( null(N{i}) ); 71 | end 72 | end -------------------------------------------------------------------------------- /v3/identifiability/RPNA_URDF_Example.m: -------------------------------------------------------------------------------- 1 | clear all; clc; 2 | 3 | % Import model via URDF 4 | floating_base_flag = 0; % Fixed base robot 5 | model = URDF_to_spatialv2_model('panda_arm_no_fixed.urdf'); 6 | model.gravity = [0 0 -9.81]'; 7 | 8 | %% Compute Parameter Nullspace with RPNA 9 | fprintf('Running RPNA\n'); 10 | [N, M, V, C] = RPNA(model, floating_base_flag); 11 | [Null_Basis, Minimal_Basis, Perp_Basis, Perp_Basis_sym] = ComputeBases(model, N, M); 12 | 13 | % Compute identifiable parameter combinations from the basis for the 14 | % subspace perpendicular to the parameter nullspace 15 | Perp_Basis_sym = rref(Perp_Basis_sym')'; 16 | Perp_Basis = rref(Perp_Basis')'; 17 | 18 | %% Print out parameter regroupings 19 | fprintf(1,'===================================\n'); 20 | fprintf(1,'Minimal Parameter Detail\n'); 21 | fprintf(1,'===================================\n'); 22 | fprintf('Note 1: The listed linear cobminations of parameters are identifiable\n'); 23 | fprintf('from fully exciting data. These regroupings are also called minimal\n'); 24 | fprintf('parameters or base parameters in the literature. \n\n'); 25 | 26 | fprintf('Note 2: More geometrically, the regrouping coefficients for each \n') 27 | fprintf('base parameter provide a basis vector for the orthogonal complement to \n') 28 | fprintf('the parameter nullspace \\mathcal{N}. Since the choice of a basis for \n') 29 | fprintf('a vector subspace is not unique, it follows that the choice of base\n'); 30 | fprintf('parameters is not unique either\n\n'); 31 | 32 | fprintf('Note 3: The URDF has a separate link frame at each link CoM and with two frames \n') 33 | fprintf('implied around each joint. If Joint i connects body i to its parent, then frame i+ \n') 34 | fprintf('is immediately after the joint and is connected to body i, with i- immediately\n') 35 | fprintf('before the joint and connected to the parent body. All quanities in spatial_v2 \n') 36 | fprintf('are given in the i+ frames. As such the regrouped parameters are given relative \n') 37 | fprintf('to these frames as well.\n\n') 38 | 39 | param_names = {'m', 'mcx', 'mcy', 'mcz', 'Ixx', 'Iyy', 'Izz', 'Iyz', 'Ixz', 'Ixy'}; 40 | 41 | % Create variables for printing parameter regroupings 42 | sym_params = sym( zeros(10*model.NB,1) ) ; 43 | for i = 1:model.NB 44 | for k = 1:10 45 | sym_params(10*i-10 + k ) = sym(sprintf('%s%d',param_names{k},i)); 46 | end 47 | end 48 | 49 | sympref('FloatingPointOutput',true); 50 | for i = 1:size(Perp_Basis_sym,2) 51 | ind = find(Perp_Basis_sym(:,i)==1,1); 52 | 53 | % Identifable parameter combination 54 | sym_result = Perp_Basis_sym(:,i)'*sym_params; 55 | 56 | % Work to strip out zero coefficients 57 | [coef, monomials] = coeffs(sym_result); 58 | coef = CleanMat(coef); 59 | sym_result = simplify( sum( coef(:).*monomials(:) ) ); 60 | 61 | % And then group terms that multiply each parameter 62 | sym_result = jacobian(sym_result, sym_params)*sym_params; 63 | fprintf(1,'Regrouped parameter %s <= ', char(sym_params(ind))); 64 | disp(sym_result) 65 | end 66 | 67 | sympref('FloatingPointOutput','default'); 68 | fprintf('\nNullspace Dimension RPNA %d\n',size(Null_Basis,2)) 69 | fprintf('Identifiable Dimension %d\n',size(Perp_Basis,2)) 70 | 71 | fprintf(1,'\n'); -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/AutoGenerated/CreateAutoGeneratedFunctions.m: -------------------------------------------------------------------------------- 1 | % There are three main function types created: 2 | % 1) Output functions: These provide some output, linear in the inertial 3 | % paramters. For instance, given some velocities v1, v2 one such output 4 | % gives y = v1'*I*v2 expressed linearly w.r.t. inertial paramters a. 5 | % 6 | % 2) Rate functions: These provide the rate of change in some parameters 7 | % as a function of an input velocity. 8 | % 9 | % 3) Transform functions: These propogate parameters across a link as a 10 | % function of an input spatial transform. 11 | % 12 | % Create a bunch of symbolic variables that will help us to write support 13 | % functions using matlabFunction. 14 | clear 15 | syms m mcx mcy mcz Ixx Iyy Izz Iyz Ixz Ixy real 16 | a = [m mcx mcy mcz Ixx Iyy Izz Iyz Ixz Ixy]'; % Inertial parameter vector 17 | a2 = sym('a2',[10 1],'real'); 18 | 19 | v1 = sym('v1',[6,1],'real'); vv1 = sym('vv1',[12,1],'real'); 20 | v2 = sym('v2',[6,1],'real'); vv2 = sym('vv2',[12,1],'real'); 21 | 22 | X = sym('X',[6,6],'real'); % Mock spatial transform matrix 23 | K = sym('Q',[6,6],'real'); % Mock matrix for outputs of form tr( K * I) 24 | d = sym('d',[10 1],'real'); % Vector of dual inertial parameters 25 | 26 | I = inertiaVecToMat(a); % 6x6 spatial inertia 27 | I2= inertiaVecToMat(a2); 28 | 29 | %% Create Functions for inertial parameters 30 | y1= v1'*I*v2; % Output for inner product between v1 and v2 31 | C1 = jacobian(y1,a); % Linear relation: y1 = C1 * a 32 | matlabFunction(C1,'File','Output_InnerProduct','vars', {v1,v2}); 33 | 34 | y2 = vv1'*[I zeros(6) ; zeros(6) I2]*vv2; % Likewise for motors 35 | C2 = jacobian(y2,[a' a2']'); % Linear relation: y2 = C2 * [a ; a2] 36 | matlabFunction(C2,'File','Output_InnerProduct_rotor','vars', {vv1,vv2}); 37 | 38 | a_dot = inertiaMatToVec(crf(v1)*I - I*crm(v1)); % Rate of change in inertia paramters when moving with v1 39 | A_phi = jacobian(a_dot , a); % Linear relation: a_dot = A_phi * a 40 | matlabFunction(A_phi,'File','Rate_Parameters','vars',{v1}); 41 | 42 | a_trans = inertiaMatToVec(X' * I * X); % Parameters after transformation 43 | A_X = jacobian(a_trans, a); % Linear relation: a_trans = A_X * a 44 | matlabFunction(A_X,'File','Transform_Parameters','vars',{X}); -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/AutoGenerated/Output_InnerProduct.m: -------------------------------------------------------------------------------- 1 | function C1 = Output_InnerProduct(in1,in2) 2 | %OUTPUT_INNERPRODUCT 3 | % C1 = OUTPUT_INNERPRODUCT(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 7.1. 6 | % 19-Dec-2017 16:00:33 7 | 8 | v11 = in1(1,:); 9 | v12 = in1(2,:); 10 | v13 = in1(3,:); 11 | v14 = in1(4,:); 12 | v15 = in1(5,:); 13 | v16 = in1(6,:); 14 | v21 = in2(1,:); 15 | v22 = in2(2,:); 16 | v23 = in2(3,:); 17 | v24 = in2(4,:); 18 | v25 = in2(5,:); 19 | v26 = in2(6,:); 20 | C1 = [v14.*v24+v15.*v25+v16.*v26,-v12.*v26+v13.*v25+v15.*v23-v16.*v22,v11.*v26-v13.*v24-v14.*v23+v16.*v21,-v11.*v25+v12.*v24+v14.*v22-v15.*v21,v11.*v21,v12.*v22,v13.*v23,v12.*v23+v13.*v22,v11.*v23+v13.*v21,v11.*v22+v12.*v21]; 21 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/AutoGenerated/Output_InnerProduct_rotor.m: -------------------------------------------------------------------------------- 1 | function C2 = Output_InnerProduct_rotor(in1,in2) 2 | %OUTPUT_INNERPRODUCT_MOTOR 3 | % C2 = OUTPUT_INNERPRODUCT_MOTOR(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 7.1. 6 | % 19-Dec-2017 16:00:34 7 | 8 | vv11 = in1(1,:); 9 | vv12 = in1(2,:); 10 | vv13 = in1(3,:); 11 | vv14 = in1(4,:); 12 | vv15 = in1(5,:); 13 | vv16 = in1(6,:); 14 | vv17 = in1(7,:); 15 | vv18 = in1(8,:); 16 | vv19 = in1(9,:); 17 | vv21 = in2(1,:); 18 | vv22 = in2(2,:); 19 | vv23 = in2(3,:); 20 | vv24 = in2(4,:); 21 | vv25 = in2(5,:); 22 | vv26 = in2(6,:); 23 | vv27 = in2(7,:); 24 | vv28 = in2(8,:); 25 | vv29 = in2(9,:); 26 | vv110 = in1(10,:); 27 | vv111 = in1(11,:); 28 | vv112 = in1(12,:); 29 | vv210 = in2(10,:); 30 | vv211 = in2(11,:); 31 | vv212 = in2(12,:); 32 | C2 = [vv14.*vv24+vv15.*vv25+vv16.*vv26,-vv12.*vv26+vv13.*vv25+vv15.*vv23-vv16.*vv22,vv11.*vv26-vv13.*vv24-vv14.*vv23+vv16.*vv21,-vv11.*vv25+vv12.*vv24+vv14.*vv22-vv15.*vv21,vv11.*vv21,vv12.*vv22,vv13.*vv23,vv12.*vv23+vv13.*vv22,vv11.*vv23+vv13.*vv21,vv11.*vv22+vv12.*vv21,vv110.*vv210+vv111.*vv211+vv112.*vv212,-vv28.*vv112+vv29.*vv111-vv18.*vv212+vv19.*vv211,vv27.*vv112-vv29.*vv110+vv17.*vv212-vv19.*vv210,-vv27.*vv111+vv28.*vv110-vv17.*vv211+vv18.*vv210,vv17.*vv27,vv18.*vv28,vv19.*vv29,vv18.*vv29+vv19.*vv28,vv17.*vv29+vv19.*vv27,vv17.*vv28+vv18.*vv27]; 33 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/AutoGenerated/Rate_Parameters.m: -------------------------------------------------------------------------------- 1 | function A_phi = Rate_Parameters(in1) 2 | %RATE_PARAMETERS 3 | % A_PHI = RATE_PARAMETERS(IN1) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 7.1. 6 | % 19-Dec-2017 16:00:34 7 | 8 | v11 = in1(1,:); 9 | v12 = in1(2,:); 10 | v13 = in1(3,:); 11 | v14 = in1(4,:); 12 | v15 = in1(5,:); 13 | v16 = in1(6,:); 14 | t2 = v16.*2.0; 15 | t3 = v14.*2.0; 16 | t4 = v15.*2.0; 17 | t5 = v12.*2.0; 18 | A_phi = reshape([0.0,v14,v15,v16,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,v13,-v12,0.0,t3,t3,0.0,-v16,-v15,0.0,-v13,0.0,v11,t4,0.0,t4,-v16,0.0,-v14,0.0,v12,-v11,0.0,t2,t2,0.0,-v15,-v14,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-v12,v13,0.0,0.0,0.0,0.0,0.0,0.0,0.0,v11,0.0,-v13,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-v11,v12,0.0,0.0,0.0,0.0,0.0,0.0,v11.*-2.0,v11.*2.0,0.0,-v13,v12,0.0,0.0,0.0,0.0,t5,0.0,-t5,v13,0.0,-v11,0.0,0.0,0.0,0.0,v13.*-2.0,v13.*2.0,0.0,-v12,v11,0.0],[10,10]); 19 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/CleanMat.m: -------------------------------------------------------------------------------- 1 | function CleanA = CleanMat(A) 2 | %% CleanA = CleanMat(A) 3 | % set values close to 0,1, and -1 to the exact value. 4 | 5 | inds = find(abs(A) < 1e-9); A(inds) = 0; 6 | inds = find(abs(A-1) < 1e-9); A(inds) = 1; 7 | inds = find(abs(A+1) < 1e-9); A(inds) = -1; 8 | 9 | CleanA = A; 10 | 11 | 12 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/ComputeSampledRegressor.m: -------------------------------------------------------------------------------- 1 | function Ystack = ComputeSampledRegressor( model, samples,use_HandG_regressor) 2 | %% Ystack = ComputeSampledRegressor( model, samples) 3 | % Computes a given number of random samples of the classical regressor Y 4 | 5 | Ystack = []; 6 | for i = 1:samples 7 | qr = rand(model.NB,1); 8 | qdr = rand(model.NB,1); 9 | qddr = rand(model.NB,1); 10 | if nargin == 3 && use_HandG_regressor 11 | % The "Position Regressor" gives the regressor for the gravity 12 | % vector and the entries of the masss matrix. 13 | [Yh, Yg, Yh_rot, Yg_rot] = RegressorHandG(model, qr); 14 | Y = [Yh Yh_rot; Yg Yg_rot]; 15 | else 16 | [Yc, Yc_rot] = RegressorClassical(model,qr,qdr,qddr); 17 | Y = [Yc Yc_rot]; 18 | end 19 | Ystack = [Ystack ; Y]; 20 | end 21 | end 22 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/IsIdentifiable.m: -------------------------------------------------------------------------------- 1 | function bool = IsIdentifiable(model,N, i, k) 2 | %% bool = IsIdentifiable(model,N, i, k) 3 | % Determine if parameter k of body i is identifiable 4 | % Requires the nullspace descriptor array N from the RPNA 5 | 6 | R = null(N{i}); 7 | pi = zeros(10,1); 8 | pi(k) = 1; 9 | 10 | if norm( pi' * R ) > eps^.75 11 | bool = false; 12 | return 13 | end 14 | 15 | for j = 1:model.NB 16 | if i == model.parent(j) 17 | Rj = null(N{j}); 18 | AX = Transform_Parameters( model.Xtree{j} ); 19 | 20 | if norm( pi'*(AX*Rj) ) > eps^.75 21 | bool=false; 22 | return 23 | end 24 | end 25 | end 26 | 27 | bool = true; 28 | end -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/IsIdentifiable_rotor.m: -------------------------------------------------------------------------------- 1 | function bool = IsIdentifiable_motor(model,N, i, k) 2 | %% bool = IsIdentifiable_motor(model,N, i, k) 3 | % Determine if parameter k of body i is identifiable 4 | % if i > model.NB, then body i refers to motor (i - model.NB) 5 | % Requires the nullspace descriptor array N from the RPNA 6 | 7 | if i <= model.NB 8 | R = null(N{i}); 9 | pi = zeros(20,1); 10 | pi(k) = 1; 11 | 12 | if norm( pi' * R ) > eps^.75 13 | bool = false; 14 | return 15 | end 16 | 17 | for j = 1:model.NB 18 | pi = pi(1:10); 19 | if i == model.parent(j) 20 | Rj = null(N{j}); 21 | 22 | AX = Transform_Parameters( model.Xtree{j} ); 23 | AX_rotor = Transform_Parameters( model.Xrotor{j} ); 24 | 25 | if norm( pi'*([AX AX_rotor*model.rotor_constraint{i}]*Rj) ) > eps^.75 26 | bool=false; 27 | return 28 | end 29 | end 30 | end 31 | 32 | bool = true; 33 | else 34 | R = null(N{i-model.NB}); 35 | pi = zeros(20,1); 36 | pi(10+k) = 1; 37 | 38 | if norm( pi' * R ) > eps^.75 39 | bool = false; 40 | return 41 | end 42 | bool = true; 43 | end 44 | end 45 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/IsUnidentifiable.m: -------------------------------------------------------------------------------- 1 | function bool = IsUnidentifiable(model,N, i, k) 2 | %% bool = IsUnidentifiable(model,N, i, k) 3 | % Determine if parameter k of body i is unidentifiable 4 | % Requires the nullspace descriptor array N from the RPNA 5 | 6 | pi = zeros(10,1); 7 | pi(k) = 1; 8 | bool = true; 9 | 10 | while i > 0 11 | if norm( N{i}* pi) > eps^.75 12 | bool = false; 13 | return 14 | end 15 | X = model.Xtree{i} ; 16 | pi = inertiaMatToVec( X' * inertiaVecToMat(pi) * X ) ; 17 | i = model.parent(i); 18 | end 19 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/IsUnidentifiable_rotor.m: -------------------------------------------------------------------------------- 1 | function bool = IsUnidentifiable_rotor(model,N, i, k) 2 | %% bool = IsUnidentifiable_rotor(model,N, i, k) 3 | % Determine if parameter k of body i is unidentifiable 4 | % if i > model.NB, then body i refers to motor (i - model.NB) 5 | % Requires the nullspace descriptor array N from the RPNA 6 | 7 | pi = zeros(20,1); 8 | if i <= model.NB 9 | pi(k) = 1; 10 | else 11 | pi(k+10) = 1; 12 | if norm( N{i-model.NB}* pi) > eps^.75 13 | bool = false; 14 | return 15 | end 16 | ii = i - model.NB; 17 | X = model.Xtree{ii} ; 18 | X_rotor = model.Xrotor{ii}; 19 | AX = Transform_Parameters( X); 20 | AX_rotor = Transform_Parameters( X_rotor); 21 | pi = [ [AX AX_rotor]*pi ; zeros(10,1) ]; 22 | i = i - model.NB; 23 | end 24 | 25 | bool = true; 26 | while i > 0 27 | if norm( N{i}* pi) > eps^.75 28 | bool = false; 29 | return 30 | end 31 | X = model.Xtree{i} ; 32 | X_rotor = model.Xrotor{i}; 33 | AX = Transform_Parameters( X); 34 | AX_rotor = Transform_Parameters( X_rotor); 35 | pi = [ [AX AX_rotor]*pi ; zeros(10,1) ]; 36 | i = model.parent(i); 37 | end 38 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/OutputMatrix.m: -------------------------------------------------------------------------------- 1 | function C = OutputMatrix(L1, L2) 2 | %% C = OutputMatrix(L1, L2) 3 | % Gives the output matrix C(L1, L2) described in the accompanying paper 4 | % each column of L1 (L2) is a vector l1 (l2) 5 | % each row of C, cj, satisfies cj'*a = l1'*I*l2 for some l1, l2 6 | % where a and I are the paried parameter vector and matrix form of any spatial inertia 7 | 8 | if all( size(L1) == size(L2) ) 9 | if norm(L1-L2,'fro') < eps 10 | L1 = RangeBasis(L1); 11 | L2 = L1; 12 | else 13 | L1 = RangeBasis(L1); 14 | L2 = RangeBasis(L2); 15 | end 16 | else 17 | L1 = RangeBasis(L1); 18 | L2 = RangeBasis(L2); 19 | end 20 | C = OutputMatrix_NoReduce(L1,L2); 21 | C = RangeBasis(C')'; 22 | end 23 | 24 | function C = OutputMatrix_NoReduce(L1, L2) 25 | C = zeros(0,10); 26 | for i = 1:size(L1,2) 27 | li = L1(:,i); 28 | for j = 1:size(L2,2) 29 | lj = L2(:,j); 30 | C = [C; Output_InnerProduct(li,lj)]; 31 | end 32 | end 33 | end 34 | 35 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/OutputMatrix_rotor.m: -------------------------------------------------------------------------------- 1 | function C = OutputMatrix_rotor(L1, L2) 2 | %% C = OutputMatrix_rotor(L1, L2) 3 | % Generalization of OutputMatrix for the case of a link+motor 4 | % 5 | C = zeros(0,20); 6 | for i = 1:size(L1,2) 7 | li = L1(:,i); 8 | for j = 1:size(L2,2) 9 | lj = L2(:,j); 10 | C = [C; Output_InnerProduct_rotor(li,lj)]; 11 | end 12 | end 13 | C = RangeBasis(C')'; 14 | end -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/PrintParameterSummary.m: -------------------------------------------------------------------------------- 1 | function PrintParameterSummary(model, N, M, V, C, a) 2 | %% PrintParameterSummary(model, N, M, V, C, a) 3 | % Prints a parameter summary from the outputs of the RPNA algorithm 4 | 5 | for i = 1:model.NB 6 | 7 | fprintf('===============================\n'); 8 | fprintf('Minimal Parameters for Body %d\n',i); 9 | x=0; 10 | for k = 1:10 11 | if norm(M{i}(k,:)) > 0 12 | fprintf('%s\n',a{k}); 13 | x=x+1; 14 | end 15 | end 16 | if x == 0 17 | fprintf('None\n'); 18 | end 19 | 20 | fprintf('\nIdentifiable Parmeters for Body %d\n',i); 21 | x=0; 22 | for k = 1:10 23 | if IsIdentifiable(model,N,i,k) 24 | fprintf('%s\n',a{k}); 25 | x=x+1; 26 | end 27 | end 28 | if x == 0 29 | fprintf('None\n'); 30 | end 31 | 32 | 33 | fprintf('\nUnidentifiable Parmeters for Body %d\n',i); 34 | x=0; 35 | for k = 1:10 36 | if IsUnidentifiable(model,N,i,k) 37 | fprintf('%s\n',a{k}); 38 | x=x+1; 39 | end 40 | end 41 | if x == 0 42 | fprintf('None\n'); 43 | end 44 | 45 | fprintf('\nDim Null N(%d) = %d\n',i,10-size(N{i},1)); 46 | fprintf('Dim VelocitySpan V(%d) = %d\n',i, rank(V{i})); 47 | fprintf('Dim OuterProductSpan K(%d) = %d\n\n',i, rank(C{i})); 48 | 49 | end -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/PrintParameterSummary_rotor.m: -------------------------------------------------------------------------------- 1 | function PrintParameterSummary_motor(model, N, M, V, C, a) 2 | %% PrintParameterSummary_motor(model, N, M, V, C, a) 3 | % Prints a parameter summary from the outputs of the RPNA algorithm 4 | 5 | for i = 1:model.NB 6 | fprintf('===============================\n'); 7 | fprintf('Minimal Parameters for Body %d\n',i); 8 | x = 0; 9 | for k = 1:10 10 | if norm(M{i}(k,:)) > 0 11 | fprintf('%s\n',a{k}); 12 | x = x+1; 13 | end 14 | end 15 | for k = 1:10 16 | if norm(M{i}(k+10,:)) > 0 17 | fprintf('Motor %s\n',a{k}); 18 | x = x+1; 19 | end 20 | end 21 | if x == 0 22 | fprintf('None\n'); 23 | end 24 | 25 | x=0; 26 | fprintf('\nIdentifiable Parmeters for Body %d\n',i); 27 | for k = 1:10 28 | if IsIdentifiable_rotor(model,N,i,k) 29 | fprintf('%s\n',a{k}); 30 | x=x+1; 31 | end 32 | end 33 | 34 | for k = 1:10 35 | if IsIdentifiable_rotor(model,N,i+model.NB,k) 36 | fprintf('Motor %s\n',a{k}); 37 | x=x+1; 38 | end 39 | end 40 | if x == 0 41 | fprintf('None\n'); 42 | end 43 | 44 | 45 | x=0; 46 | fprintf('\nUnidentifiable Parmeters for Body %d\n',i); 47 | for k = 1:10 48 | if IsUnidentifiable_rotor(model,N,i,k) 49 | fprintf('%s\n',a{k}); 50 | x=x+1; 51 | end 52 | end 53 | 54 | for k = 1:10 55 | if IsUnidentifiable_rotor(model,N,i+model.NB,k) 56 | fprintf('Motor %s\n',a{k}); 57 | x=x+1; 58 | end 59 | end 60 | if x == 0 61 | fprintf('None\n'); 62 | end 63 | 64 | 65 | fprintf('\nDim Null N(%d) = %d\n',i,20-size(N{i},1)); 66 | fprintf('Rank VelocitySpan V(%d) = %d\n',i, rank(V{i})); 67 | fprintf('Rank OuterProductSpan K(%d) = %d\n\n',i, rank(C{i})); 68 | 69 | end -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/RangeBasis.m: -------------------------------------------------------------------------------- 1 | function [A, cA] = RangeBasis(A,clear) 2 | %% Gives a full rank matrix with the same columnspan as the input 3 | 4 | persistent maxCond 5 | if isempty(maxCond) 6 | maxCond = 0; 7 | end 8 | if nargin == 2 9 | maxCond = 0; 10 | end 11 | [U E V] = svd(A); 12 | if min(size(E)) > 1 13 | E = diag(E); 14 | else 15 | if min(size(E)) > 0 16 | E = E(1,1); 17 | else 18 | E = []; 19 | end 20 | end 21 | r = sum( E > eps^.75 ); 22 | if r == 0 23 | A = A(:,[]); 24 | else 25 | c = E(1) / E(r); 26 | maxCond = max(c, maxCond); 27 | A = U(:,1:r); 28 | end 29 | cA = maxCond; 30 | 31 | [Q, ~,~] = qr(A); 32 | A = Q(:,1:rank(A)); 33 | end 34 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/SwitchedControllabilityMatrix.m: -------------------------------------------------------------------------------- 1 | function R = SwitchedControllabilityMatrix(A,B) 2 | %% R = SwitchedControllabilityMatrix(A,B) 3 | % Compute the switched Controllability matrix associated with the pairs 4 | % (A_i, B) where A_i are the elements of the cell array A. 5 | 6 | R = RangeBasis(B); % Range of R gives Reachable Subspace 7 | r = 0; 8 | % While subspace dimension is growing 9 | while rank(R) > r 10 | r = rank(R); 11 | R_new = R; 12 | for i = 1:length(A) 13 | R_new = [R_new, A{i}*R]; 14 | end 15 | R = RangeBasis(R_new); 16 | end 17 | end 18 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/SwitchedObservabilityMatrix.m: -------------------------------------------------------------------------------- 1 | function N = SwitchedObservabilityMatrix(C,A) 2 | %% N = SwitchedObservabilityMatrix(C,A) 3 | % Compute the switched Observability matrix associated with the pairs 4 | % (C, A_i) where A_i are the elements of the cell array A. 5 | 6 | AT = A; 7 | for i = 1:length(A) 8 | AT{i} = A{i}'; 9 | end 10 | 11 | %Duality! 12 | N = SwitchedControllabilityMatrix(AT,C')'; 13 | end 14 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/UnitVectorComplementarySubspace.m: -------------------------------------------------------------------------------- 1 | function Comp = UnitVectorComplementarySubspace(S) 2 | %% Comp = UnitVectorComplementarySubspace(S) 3 | % Given a matrix S whose columns span a subspace, the output Comp 4 | % provides a matrix of unit vectors whose columns span a complmementary subspace 5 | 6 | n = size(S,1); 7 | m1 = size(S,2); 8 | m2 = n - size(S,2); 9 | Comp = zeros(n,m2); 10 | k = 0; 11 | for i = 1:n 12 | ei = zeros(n,1); 13 | ei(i) = 1; 14 | Comp(:,k+1) = ei; 15 | if rank([S Comp]) == m1+k+1 16 | k = k+1; 17 | if k == m2 18 | break 19 | end 20 | end 21 | end 22 | 23 | -------------------------------------------------------------------------------- /v3/identifiability/SupportFunctions/unitVector.m: -------------------------------------------------------------------------------- 1 | function e = unitVector(i,n) 2 | %% e = unitVector(i,n) 3 | % gives the unit vector such that length(e) = n and e_i = 1 4 | e = zeros(n,1); 5 | e(i) = 1; 6 | end 7 | -------------------------------------------------------------------------------- /v3/inertia/entropicDivergence.m: -------------------------------------------------------------------------------- 1 | function [f, g, H] = entropicDivergence(p, q) 2 | P = inertiaVecToPinertia(p); 3 | Q = inertiaVecToPinertia(q); 4 | f = logdetBregmanDivergence( P , Q ); 5 | if nargout > 1 6 | iQ = inv(Q); 7 | iP = inv(P); 8 | G = iQ-iP; 9 | H = zeros(10,10); 10 | g = zeros(10,1); 11 | for i = 1:10 12 | ei = zeros(10,1); 13 | ei(i) = 1; 14 | Ji = inertiaVecToPinertia(ei); 15 | g(i) = trace( Ji * G ); 16 | if nargout > 2 17 | for j=i:10 18 | ej = zeros(10,1); 19 | ej(j) = 1; 20 | Jj = inertiaVecToPinertia(ej); 21 | H(i,j) = trace(iP*Ji*iP*Jj); 22 | H(j,i) = H(i,j); 23 | end 24 | end 25 | end 26 | end 27 | end -------------------------------------------------------------------------------- /v3/inertia/getModelInertialParams.m: -------------------------------------------------------------------------------- 1 | function [a, a_rotor] = getModelInertialParams(model) 2 | if ~isfield(model,'nq') 3 | model = postProcessModel(model); 4 | end 5 | a = []; 6 | a_rotor = []; 7 | for i = 1:model.NB 8 | a = [a ; inertiaMatToVec( model.I{i} )]; 9 | if model.has_rotor(i) 10 | a_rotor = [a_rotor ; inertiaMatToVec( model.I_rotor{i} ) ]; 11 | end 12 | end 13 | end -------------------------------------------------------------------------------- /v3/inertia/inertiaMatToPinertia.m: -------------------------------------------------------------------------------- 1 | function [ Pinertia ] = inertiaMatToPinertia( I ) 2 | % inertiaMatToPinertia converts a 6x6 spatial inertia matrix to a 4x4 3 | % pseudo-inertia matrix 4 | % 5 | % Input: I (6x6) spatial inertia matrix 6 | % Output: J (4x4) Pseudo-inertia matrix 7 | % 8 | % The pseudo-inertia is discussed further in 9 | % Linear Matrix Inequalities for Physically Consistent Inertial Parameter 10 | % Identification: A Statistical Perspective on the Mass Distribution, by 11 | % Wensing, Kim, Slotine 12 | 13 | h = skew(I(1:3,4:6)); 14 | Ibar = I(1:3,1:3); 15 | m = I(6,6); 16 | Pinertia = [ 1/2*trace(Ibar)*eye(3)-Ibar h ; h.' m ]; 17 | end 18 | 19 | -------------------------------------------------------------------------------- /v3/inertia/inertiaMatToVec.m: -------------------------------------------------------------------------------- 1 | function [ a ] = inertiaMatToVec( I ) 2 | % inertiaMatToVec converts a 6x6 spatial inertia matrix into a vector of 3 | % 10 inertial parameters 4 | % 5 | % Input: I (6x6) spatial inertia matrix 6 | % 7 | % Outpt: a (10x1) Vector of inertial parameters 8 | % a = [m hx hy hz Ixx Iyy Izz Iyz Ixz Ixy]' 9 | % 10 | 11 | h = inv_skew(I(1:3,4:6)); 12 | a = [I(6,6) h(1) h(2) h(3) I(1,1) I(2,2) I(3,3) I(3,2) I(3,1) I(2,1) ].'; 13 | end 14 | 15 | -------------------------------------------------------------------------------- /v3/inertia/inertiaVecToMat.m: -------------------------------------------------------------------------------- 1 | function [ I ] = inertiaVecToMat( a ) 2 | % inertiaVecToMat converts a vector of 10 inertial parameters to 6x6 spatial 3 | % inertia matrix 4 | % 5 | % Input: a (10x1) Vector of inertial parameters 6 | % a = [m hx hy hz Ixx Iyy Izz Iyz Ixz Ixy]' 7 | % 8 | % Ouput: I (6x6) spatial inertia matrix 9 | % 10 | 11 | Ibar = [a(5) a(10) a(9) ; 12 | a(10) a(6) a(8) ; 13 | a(9) a(8) a(7) ]; 14 | h = [a(2) a(3) a(4)].'; 15 | m = a(1); 16 | I = [Ibar skew(h) ; skew(h).' m*eye(3)]; 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /v3/inertia/inertiaVecToPinertia.m: -------------------------------------------------------------------------------- 1 | function [ Pinertia ] = inertiaVecToPinertia( a ) 2 | % pinertiaToVec converts a vector of 10 inertial parameters to 4x4 3 | % Pseudo-Inertia matrix. 4 | % 5 | % Input: J (4x4) psuedo inertia 6 | % Output: a (10x1) Vector of inertial parameters 7 | % a = [m hx hy hz Ixx Iyy Izz Iyz Ixz Ixy]' 8 | % 9 | % The pseudo-inertia is discussed further in: 10 | % Linear Matrix Inequalities for Physically Consistent Inertial Parameter 11 | % Identification: A Statistical Perspective on the Mass Distribution, by 12 | % Wensing, Kim, Slotine 13 | 14 | I = inertiaVecToMat(a); 15 | h = skew(I(1:3,4:6)); 16 | Ibar = I(1:3,1:3); 17 | m = I(6,6); 18 | Pinertia = [ 1/2*trace(Ibar)*eye(3)-Ibar h ; h.' m ]; 19 | end 20 | 21 | -------------------------------------------------------------------------------- /v3/inertia/pinertiaToVec.m: -------------------------------------------------------------------------------- 1 | function [ a ] = pinertiaToVec( J ) 2 | % pinertiaToVec converts a 4x4 Pseudo-Inertia matrix to a vector of 10 3 | % inertial parameters. 4 | % 5 | % Input: J (4x4) psuedo inertia 6 | % Output: a (10x1) Vector of inertial parameters 7 | % a = [m hx hy hz Ixx Iyy Izz Ixy Ixz Iyz]' 8 | % 9 | % The pseudo-inertia is discussed further in 10 | % Linear Matrix Inequalities for Physically Consistent Inertial Parameter 11 | % Identification: A Statistical Perspective on the Mass Distribution, by 12 | % Wensing, Kim, Slotine 13 | 14 | m = J(4,4); 15 | h = J(1:3,4); 16 | c = h/m; 17 | E = J(1:3,1:3); 18 | Ibar = trace(E)*eye(3) - E; 19 | 20 | a = inertiaMatToVec([Ibar skew(h) ; skew(h).' m*eye(3)]); 21 | end 22 | 23 | -------------------------------------------------------------------------------- /v3/inertia/pullbackMetric.m: -------------------------------------------------------------------------------- 1 | function [M] = pullbackMetric(params) 2 | M = zeros(10,10); 3 | P = inertiaVecToPinertia(params); 4 | P_inv = inv(P); 5 | v_i = zeros(10,1); 6 | v_j = zeros(10,1); 7 | for i = 1 : 10 8 | for j = 1 : 10 9 | v_i = zeros(10,1); v_j = zeros(10,1); 10 | v_i(i) =1 ; v_j(j) = 1; 11 | V_i = inertiaVecToPinertia(v_i); V_j = inertiaVecToPinertia(v_j); 12 | M(i,j) = trace(P_inv*V_i*P_inv*V_j); 13 | end 14 | end 15 | end -------------------------------------------------------------------------------- /v3/models/CheetahLeg_model.m: -------------------------------------------------------------------------------- 1 | function robot = CheetahLeg_model() 2 | %% robot = CheetahLeg_model() 3 | % Creates a robot model of the Cheetah 3 that is compatible with the 4 | % spatial_v2 dynamics library. 5 | 6 | % Constraints on motor parameters to ensure rotational symmetry (about z) 7 | motor_symmetry_constraint = zeros(6,10); 8 | motor_symmetry_constraint(1,2) = 1; 9 | motor_symmetry_constraint(2,3) = 1; 10 | motor_symmetry_constraint(3,5) = 1; motor_symmetry_constraint(3,6) = -1; 11 | motor_symmetry_constraint(4,8) = 1; 12 | motor_symmetry_constraint(5,9) = 1; 13 | motor_symmetry_constraint(6,10)= 1; 14 | 15 | B = RangeBasis( null(motor_symmetry_constraint) ); 16 | P = B*B'; % Projector onto nullspace of symmetry constraint 17 | 18 | robot.gravity = [9.81 0 0]; 19 | 20 | lo = 0.044; 21 | l1 = .342; 22 | 23 | aa3 = sym('a3','real'); 24 | dd3 = sym('d3','real'); 25 | 26 | 27 | 28 | % Modified Denavit Hartenberg parameters alpha_{i-1}, a_{i-1}, and d_i 29 | % as in J.J.Craig's classic text 30 | dhTable = [0 0 0 ; 31 | pi/2 0 lo ; 32 | 0 l1 0 ]; 33 | 34 | 35 | dhTable_sym = [0 0 0 ; 36 | pi/2 0 dd3 ; 37 | 0 aa3 0 ]; 38 | 39 | 40 | robot.NB = 3; 41 | robot.parent = [0 1 2]; 42 | robot.jtype = {'Rz', 'Rz', 'Rz'}; 43 | robot.jtype_rotor = {'Rz', 'Rz', 'Rz'}; 44 | robot.has_rotor = [1 1 1]; 45 | 46 | 47 | for i = 1:3 48 | robot.gr{i} = 10; 49 | alpha = dhTable(i,1); 50 | a = dhTable(i,2); 51 | d = dhTable(i,3); 52 | 53 | robot.Xtree{i} = xlt([0 0 d]) * xlt([a 0 0]) * rotx(alpha); 54 | robot.Xtree_motor{i} = xlt([0 0 d]) * xlt([a 0 0]) * rotx(alpha); 55 | robot.motor_constraint{i} = P; 56 | 57 | 58 | alpha = dhTable(i,1); 59 | a_s = dhTable_sym(i,2); 60 | d_s = dhTable_sym(i,3); 61 | robot.Xtree_sym{i} = xlt([0 0 d_s]) * xlt([a_s 0 0]) * round(rotx(alpha)); 62 | robot.Xtree_motor_sym{i} = robot.Xtree_sym{i}; 63 | 64 | end 65 | 66 | % Knee motor sits at the hip 67 | robot.Xtree_motor{3} = eye(6); 68 | robot.I = { eye(6),eye(6),eye(6)}; 69 | robot.I_rotor = { eye(6),eye(6),eye(6)}; -------------------------------------------------------------------------------- /v3/models/Puma560_model.m: -------------------------------------------------------------------------------- 1 | function robot = Puma560_model() 2 | %% robot = Puma560_model() 3 | % Creates a robot model of the Puma560 that is compatible with the 4 | % spatial_v2 dynamics library. 5 | 6 | a2 = .2; 7 | d3 = .4; 8 | a3 = .153; 9 | d4 = .839; 10 | 11 | aa2 = sym('a2','real'); 12 | aa3 = sym('a3','real'); 13 | dd3 = sym('d3','real'); 14 | dd4 = sym('d4','real'); 15 | 16 | 17 | % Modified Denavit Hartenberg parameters alpha_{i-1}, a_{i-1}, and d_i 18 | % as in J.J.Craig's classic text 19 | dhTable = [0 0 0 ; 20 | -pi/2 0 0 ; 21 | 0 a2 d3 ; 22 | -pi/2 a3 d4 ; 23 | pi/2 0 0 ; 24 | -pi/2 0 0 ]; 25 | 26 | 27 | dhTable_sym = [0 0 0 ; 28 | -pi/2 0 0 ; 29 | 0 aa2 dd3 ; 30 | -pi/2 aa3 dd4 ; 31 | pi/2 0 0 ; 32 | -pi/2 0 0 ]; 33 | 34 | robot.NB = 6; 35 | robot.parent = [0 1 2 3 4 5]; 36 | robot.jtype = {'Rz', 'Rz', 'Rz','Rz','Rz','Rz'}; 37 | 38 | robot.jtype_rotor = {'Rz', 'Rz', 'Rz','Rz','Rz','Rz'}; 39 | robot.has_rotor = ones(6,1); 40 | 41 | for i = 1:6 42 | robot.gr{i} = 5; 43 | end 44 | 45 | 46 | % Constraints on motor parameters to ensure rotational symmetry (about z) 47 | rotor_symmetry_constraint = zeros(6,10); 48 | rotor_symmetry_constraint(1,2) = 1; 49 | rotor_symmetry_constraint(2,3) = 1; 50 | rotor_symmetry_constraint(3,5) = 1; rotor_symmetry_constraint(3,6) = -1; 51 | rotor_symmetry_constraint(4,8) = 1; 52 | rotor_symmetry_constraint(5,9) = 1; 53 | rotor_symmetry_constraint(6,10)= 1; 54 | 55 | B = RangeBasis( null(rotor_symmetry_constraint) ); 56 | P = B*B'; % Projector onto nullspace of symmetry constraint 57 | 58 | 59 | robot.Xtree = { }; 60 | for i = 1:6 61 | alpha = dhTable(i,1); 62 | a = dhTable(i,2); 63 | d = dhTable(i,3); 64 | robot.Xtree{i} = xlt([0 0 d]) * xlt([a 0 0]) * rotx(alpha); 65 | robot.Xrotor{i} = robot.Xtree{i}; 66 | 67 | 68 | alpha = dhTable(i,1); 69 | a_s = dhTable_sym(i,2); 70 | d_s = dhTable_sym(i,3); 71 | robot.Xtree_sym{i} = xlt([0 0 d_s]) * xlt([a_s 0 0]) * round(rotx(alpha)); 72 | robot.Xrotor_sym{i} = robot.Xtree_sym{i}; 73 | 74 | robot.rotor_constraint{i} = P; 75 | end 76 | 77 | robot.gravity = [0 0 -9.81]; 78 | robot.I = { eye(6),eye(6),eye(6),eye(6),eye(6),eye(6)}; 79 | robot.I_rotor = { eye(6),eye(6),eye(6),eye(6),eye(6),eye(6)}; -------------------------------------------------------------------------------- /v3/models/Scara_model.m: -------------------------------------------------------------------------------- 1 | function robot = Scara_model() 2 | %% robot = CreateScara() 3 | % Creates a robot model of the Scara that is compatible with the 4 | % spatial_v2 dynamics library. 5 | 6 | a1 = .1; 7 | a2 = .2; 8 | 9 | aa1 = sym('a1'); 10 | aa2 = sym('a2'); 11 | 12 | % Modified Denavit Hartenberg parameters alpha_{i-1}, a_{i-1}, and d_i 13 | % as in J.J.Craig's classic text 14 | dhTable = [0 0 0; 15 | 0 a1 0 ; 16 | 0 a2 0 ; 17 | 0 0 0 ; 18 | ]; 19 | 20 | 21 | dhTable_sym = [0 0 0; 22 | 0 aa1 0 ; 23 | 0 aa2 0 ; 24 | 0 0 0 ; 25 | ]; 26 | 27 | % Constraints on motor parameters to ensure rotational symmetry (about z) 28 | rotor_symmetry_constraint = zeros(6,10); 29 | rotor_symmetry_constraint(1,2) = 1; 30 | rotor_symmetry_constraint(2,3) = 1; 31 | rotor_symmetry_constraint(3,5) = 1; rotor_symmetry_constraint(3,6) = -1; 32 | rotor_symmetry_constraint(4,8) = 1; 33 | rotor_symmetry_constraint(5,9) = 1; 34 | rotor_symmetry_constraint(6,10)= 1; 35 | 36 | 37 | B = RangeBasis( null(rotor_symmetry_constraint) ); 38 | P = B*B'; % Projector onto nullspace of symmetry constraint 39 | 40 | robot.NB = 4; 41 | robot.parent = [0 1 2 3]; 42 | robot.jtype = {'Rz', 'Rz', 'Pz','Rz'}; 43 | robot.jtype_rotor = {'Rz', 'Rz', 'Rz','Rz'}; 44 | robot.has_rotor = ones(4,1); 45 | 46 | robot.gr = {5, 5,5,5}; 47 | for i = 1:4 48 | robot.gr{i} = 5; 49 | end 50 | 51 | robot.Xtree = { }; 52 | for i = 1:4 53 | alpha = dhTable(i,1); 54 | a = dhTable(i,2); 55 | d = dhTable(i,3); 56 | 57 | a_s = dhTable_sym(i,2); 58 | robot.Xtree{i} = xlt([0 0 d]) * xlt([a 0 0]) * rotx(alpha); 59 | robot.Xtree_sym{i} = xlt([a_s 0 0]) * round(rotx(alpha)); 60 | robot.Xrotor{i} = robot.Xtree{i}; 61 | robot.Xrotor_sym{i} = robot.Xtree_sym{i}; 62 | robot.rotor_constraint{i} = P; 63 | end 64 | 65 | robot.gravity = [0 0 -9.81]; 66 | robot.I = { eye(6),eye(6),eye(6),eye(6)}; 67 | robot.I_rotor = { eye(6),eye(6),eye(6),eye(6)}; -------------------------------------------------------------------------------- /v3/models/TX40_model.m: -------------------------------------------------------------------------------- 1 | function robot = TX40_model() 2 | %% robot = TX40_model() 3 | % Creates a robot model of the TX40 that is compatible with the 4 | % spatial_v2 dynamics library. 5 | 6 | d3 = .225; 7 | r3 = 0.035; 8 | r4 = .225; 9 | 10 | % Modified Denavit Hartenberg parameters alpha_{i-1}, a_{i-1}, and d_i 11 | % as in J.J.Craig's classic text 12 | dhTable = [0 0 0 ; 13 | -pi/2 0 0 ; 14 | 0 d3 r3 ; 15 | pi/2 0 r4 ; 16 | -pi/2 0 0 ; 17 | pi/2 0 0 ]; 18 | 19 | % Constraints on motor parameters to ensure rotational symmetry (about z) 20 | motor_symmetry_constraint = zeros(6,10); 21 | motor_symmetry_constraint(1,2) = 1; 22 | motor_symmetry_constraint(2,3) = 1; 23 | motor_symmetry_constraint(3,5) = 1; motor_symmetry_constraint(3,6) = -1; 24 | motor_symmetry_constraint(4,8) = 1; 25 | motor_symmetry_constraint(5,9) = 1; 26 | motor_symmetry_constraint(6,10)= 1; 27 | 28 | 29 | B = RangeBasis( null(motor_symmetry_constraint) ); 30 | P = B*B'; % Projector onto nullspace of symmetry constraint 31 | 32 | robot.NB = 6; 33 | robot.parent = [0 1 2 3 4 5]; 34 | robot.jtype = {'Rz', 'Rz', 'Rz','Rz','Rz','Rz'}; 35 | robot.jtype_rotor = {'Rz', 'Rz', 'Rz','Rz','Rz','Rz'}; 36 | robot.has_rotor = ones(6,1); 37 | 38 | robot.gr = { 32, 32, 45 , 48 , 45 , 32 }; 39 | 40 | robot.Xtree = { }; 41 | for i = 1:6 42 | alpha = dhTable(i,1); 43 | a = dhTable(i,2); 44 | d = dhTable(i,3); 45 | robot.Xtree{i} = xlt([0 0 d]) * xlt([a 0 0]) * rotx(alpha); 46 | robot.Xrotor{i} = robot.Xtree{i}; 47 | robot.motor_constraint{i} = P; 48 | end 49 | 50 | robot.gravity = [0 0 0]; % zero gravity is not the default, 51 | for i = 1:robot.NB 52 | robot.I{i} = eye(6); 53 | robot.I_rot{i} = eye(6); 54 | end 55 | -------------------------------------------------------------------------------- /v3/models/Test_model.m: -------------------------------------------------------------------------------- 1 | function robot = Test_model() 2 | 3 | % Constraints on motor parameters to ensure rotational symmetry (about z) 4 | rotor_symmetry_constraint = zeros(6,10); 5 | rotor_symmetry_constraint(1,2) = 1; 6 | rotor_symmetry_constraint(2,3) = 1; 7 | rotor_symmetry_constraint(3,5) = 1; rotor_symmetry_constraint(3,6) = -1; 8 | rotor_symmetry_constraint(4,8) = 1; 9 | rotor_symmetry_constraint(5,9) = 1; 10 | rotor_symmetry_constraint(6,10)= 1; 11 | 12 | B = RangeBasis( null(rotor_symmetry_constraint) ); 13 | P = B*B'; % Projector onto nullspace of symmetry constraint 14 | 15 | 16 | 17 | robot.NB = 2; 18 | robot.parent = [0 1]; 19 | robot.jtype = {'Rz', 'Rz'}; 20 | 21 | robot.Xtree = { eye(6), plux(expm(skew([pi/2 0 0])) , [1 0 0]) }; 22 | 23 | robot.gravity = [0 0 0]; % zero gravity is not the default, 24 | % so it must be stated explicitly 25 | 26 | robot.jtype_rotor = {'Rz', 'Rz'}; 27 | robot.has_rotor = ones(2,1); 28 | 29 | for i = 1:2 30 | robot.gr{i} = 5; 31 | robot.Xrotor{i} = robot.Xtree{i}; 32 | robot.rotor_constraint{i} = P; 33 | robot.I{i} = eye(6); 34 | robot.I_rotor{i} = eye(6); 35 | end -------------------------------------------------------------------------------- /v3/models/autoTree_rotor.m: -------------------------------------------------------------------------------- 1 | function model = autoTree_rotor( nb, bf, skew, taper ) 2 | 3 | % autoTree Create System Models of Kinematic Trees 4 | % autoTree(nb,bf,skew,taper) creates system models of kinematic trees 5 | % having revolute joints, mainly for the purpose of testing dynamics 6 | % functions. nb and bf specify the number of bodies in the tree, and the 7 | % branching factor, respectively. The latter is the average number of 8 | % children of a nonterminal node, and must be >=1. bf=1 produces an 9 | % unbranched tree; bf=2 produces a binary tree; and non-integer values 10 | % produce trees in which the number of children alternates between 11 | % floor(bf) and ceil(bf) in such a way that the average is bf. Trees are 12 | % constructed (and numbered) breadth-first. Link i is a thin-walled 13 | % cylindrical tube of length l(i), radius l(i)/20, and mass m(i), lying 14 | % between 0 and l(i) on the x axis of its local coordinate system. The 15 | % values of l(i) and m(i) are determined by the tapering coefficient: 16 | % l(i)=taper^(i-1) and m(i)=taper^(3*(i-1)). Thus, if taper=1 then 17 | % m(i)=l(i)=1 for all i. The inboard joint axis of link i lies on the 18 | % local z axis, and its outboard axis passes through the point (l(i),0,0) 19 | % and is rotated about the x axis by an angle of skew radians relative to 20 | % the inboard axis. If the link has more than one outboard joint then they 21 | % all have the same axis. If skew=0 then the mechanism is planar. The 22 | % final one, two or three arguments can be omitted, in which case they 23 | % assume default values of taper=1, skew=0 and bf=1. 24 | 25 | if nargin < 4 26 | taper = 1; 27 | end 28 | if nargin < 3 29 | skew = 0; 30 | end 31 | if nargin < 2 32 | bf = 1; 33 | end 34 | 35 | model.NB = nb; 36 | 37 | for i = 1:nb 38 | model.jtype{i} = 'Rz'; 39 | model.jtype_rotor{i} = 'Rx'; 40 | 41 | model.parent(i) = floor((i-2+ceil(bf))/bf); 42 | if model.parent(i) == 0 43 | model.Xtree{1} = xlt([0 0 0]); 44 | model.Xrotor{1} = xlt([0 0 0]); 45 | 46 | else 47 | model.Xtree{i} = rotx(skew) * xlt([len(model.parent(i)),0,0]); 48 | model.Xrotor{i} = rotx(skew/2) * xlt([0,len(model.parent(i))/2,0]); 49 | end 50 | 51 | model.has_rotor(i) = 1; 52 | model.gr{i} = i; 53 | 54 | len(i) = taper^(i-1); 55 | mass = taper^(3*(i-1)); 56 | CoM = len(i) * [0.5,0,0]; 57 | Icm = mass * len(i)^2 * diag([0.0025,1.015/12,1.015/12]); 58 | model.I{i} = mcI( mass, CoM, Icm ); 59 | model.I_rotor{i} = mcI( mass/3, CoM*0, Icm/3 ); 60 | 61 | end 62 | 63 | % drawing instructions 64 | 65 | model.appearance.base = ... 66 | { 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.06] }; 67 | 68 | p0 = -1; 69 | for i = 1:nb 70 | p1 = model.parent(i); 71 | tap = taper^(i-1); 72 | if p1 == 0 73 | ptap = 1; 74 | else 75 | ptap = taper^(p1-1); 76 | end 77 | if ( p1 > p0 ) 78 | model.appearance.body{i} = ... 79 | { 'cyl', [0 0 0; 1 0 0]*tap, 0.05*tap, ... 80 | 'cyl', [0 0 -0.07; 0 0 0.07]*ptap, 0.08*ptap }; 81 | p0 = p1; 82 | else 83 | model.appearance.body{i} = ... 84 | { 'cyl', [0 0 0; 1 0 0]*tap, 0.05*tap }; 85 | end 86 | end 87 | -------------------------------------------------------------------------------- /v3/models/panda_basis_from_DH.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ROAM-Lab-ND/spatial_v2_extended/881988b948f1f1b19262afebbcf56abed300558f/v3/models/panda_basis_from_DH.mat -------------------------------------------------------------------------------- /v3/models/panda_basis_from_URDF.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ROAM-Lab-ND/spatial_v2_extended/881988b948f1f1b19262afebbcf56abed300558f/v3/models/panda_basis_from_URDF.mat -------------------------------------------------------------------------------- /v3/orientation_tools/AngleAxis/angleAxisRateLeft.m: -------------------------------------------------------------------------------- 1 | function ang_ax_dot = angleAxisRateLeft(ang_ax,w) 2 | I = eye(3); 3 | 4 | sinc2 = @(z) sinc(z/pi); 5 | 6 | phi = norm(ang_ax); 7 | ax = ang_ax / phi; 8 | if phi < sqrt(eps) 9 | ax = [1 0 0]'; 10 | end 11 | 12 | % This term is zero at 0, and has zero derivative at 0. 13 | % So when phi< sqrt(eps), this term is zero to machine precision 14 | % If you were to divide it by phi^2, then you could use ang_ax in place of 15 | % ax below 16 | term = 1-cos(phi/2)/sinc2(phi/2); 17 | 18 | ang_ax_dot = (I - 1/2*skew(ang_ax) + term*skew(ax)^2)*w; 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /v3/orientation_tools/AngleAxis/angleAxisRateRight.m: -------------------------------------------------------------------------------- 1 | function ang_ax_dot = angleAxisRateRight(ang_ax,w) 2 | I = eye(3); 3 | 4 | sinc2 = @(z) sinc(z/pi); 5 | 6 | phi = norm(ang_ax); 7 | ax = ang_ax / phi; 8 | if phi < sqrt(eps) 9 | ax = [1 0 0]'; 10 | end 11 | 12 | % This term has zero derivative at 0. So when phi< sqrt(eps), this term is zero to machine precision 13 | term = 1-cos(phi/2)/sinc2(phi/2); 14 | 15 | ang_ax_dot = (I + 1/2*skew(ang_ax) + term*skew(ax)^2)*w; 16 | 17 | end 18 | 19 | -------------------------------------------------------------------------------- /v3/orientation_tools/AngleAxis/angleAxisToAngleAndAxis.m: -------------------------------------------------------------------------------- 1 | function [angle,axis] = angleAxisToAngleAndAxis(angle_axis) 2 | 3 | angle = norm(angle_axis); 4 | if angle == 0 5 | axis = [0 0 0]'; 6 | else 7 | axis = angle_axis/angle; 8 | end 9 | 10 | end 11 | 12 | -------------------------------------------------------------------------------- /v3/orientation_tools/AngleAxis/angleAxisToCayley.m: -------------------------------------------------------------------------------- 1 | function cayley = angleAxisToCayley(ang_axis) 2 | 3 | [angle, axis] = angleAxisToAngleAndAxis(ang_axis); 4 | 5 | cayley= tan(angle/2)*axis; 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /v3/orientation_tools/AngleAxis/angleAxisToMRP.m: -------------------------------------------------------------------------------- 1 | function mrp = angleAxisToMRP(ang_axis) 2 | 3 | [angle, axis] = angleAxisToAngleAndAxis(ang_axis); 4 | mrp= tan(angle/4)*axis; 5 | 6 | end 7 | 8 | -------------------------------------------------------------------------------- /v3/orientation_tools/AngleAxis/angleAxisToQuat.m: -------------------------------------------------------------------------------- 1 | function quat = angleAxisToQuat(ang_axis) 2 | 3 | sinc2 = @(z) sinc(z/pi); 4 | 5 | %[angle, axis] = angleAxisToAngleAndAxis(ang_axis); 6 | angle = sqrt(ang_axis.'*ang_axis); 7 | quat = [cos(angle/2) ; sinc2(angle/2)*ang_axis/2]; 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /v3/orientation_tools/AngleAxis/angleAxisToRot.m: -------------------------------------------------------------------------------- 1 | function R = angleAxisToRot(angle_axis) 2 | %R = expm(skew(angle_axis)); 3 | 4 | sinc2 = @(z) sinc(z/pi); 5 | e = angle_axis; 6 | phi = sqrt(e'*e); 7 | I = eye(3); 8 | 9 | % Rodrigues formula, without having to sorry about divide by zero. 10 | R = cos(phi)*I + sinc2(phi/2)^2*e*e'/2 + sinc2(phi)*skew(e); 11 | 12 | end 13 | 14 | -------------------------------------------------------------------------------- /v3/orientation_tools/AngleAxis/angleAxisToRpy.m: -------------------------------------------------------------------------------- 1 | function rpy = angleAxisToRpy(angle_axis) 2 | 3 | rpy = rotToRpy(angleAxisToRot(angle_axis)); 4 | 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/Cayley/cayleyProduct.m: -------------------------------------------------------------------------------- 1 | function [cout] = cayleyProduct(c1, c2) 2 | cout = (c1+c2+cross(c1,c2))/(1-c1'*c2); 3 | end 4 | -------------------------------------------------------------------------------- /v3/orientation_tools/Cayley/cayleyRateLeft.m: -------------------------------------------------------------------------------- 1 | function cDot = cayleyRateLeft(c,w,n) 2 | if nargin == 2 3 | n = 1; 4 | end 5 | 6 | if n == 1 7 | I = eye(3); 8 | cDot = 1/2*( I - skew(c) + c*c')*w; 9 | else 10 | cDot = getRateLeft(c, w, n); 11 | end 12 | end 13 | 14 | %%%%% 15 | function [dtau, A] = getRateLeft(tau,w, n) 16 | for i = 1:3 17 | ei = zeros(3,1); 18 | ei(i) = 1; 19 | A(:,i) = getOmegaLeft(tau,ei,n); 20 | end 21 | dtau = A\w; 22 | end 23 | 24 | function [w, R] = getOmegaLeft(tau, dtau, n ) 25 | T = -skew(tau); 26 | dT = -skew(dtau); 27 | [sk_w, R] = derivHelper(T,dT,n); 28 | w = skew(sk_w); 29 | end 30 | 31 | function [sk_w,R] = derivHelper(T,dT,n) 32 | A = ( eye(3) - T )^n; 33 | B = ( eye(3) + T )^n; 34 | R = A/B; 35 | 36 | D1 = differ(eye(3)-T,n, -dT); 37 | D2 = R*differ(eye(3)+T,n,dT); 38 | sk_w = (D1 - D2)/A; 39 | end 40 | 41 | function D = differ(A,n, dA) 42 | D = 0*A; 43 | for j = 0:(n-1) 44 | D = D + A^j * dA * A^(n-j-1); 45 | end 46 | end 47 | 48 | -------------------------------------------------------------------------------- /v3/orientation_tools/Cayley/cayleyRateRight.m: -------------------------------------------------------------------------------- 1 | function cDot = cayleyRateRight(c,w,n) 2 | if nargin == 2 3 | n = 1; 4 | end 5 | I = eye(3); 6 | 7 | if n ==1 8 | cDot = 1/2*( I + skew(c) + c*c')*w; 9 | else 10 | cDot = getRateRight(c, w,n); 11 | end 12 | 13 | end 14 | 15 | %%%%%%%%%%%%%%%% 16 | function [dtau, A] = getRateRight(tau,w, n) 17 | for i = 1:3 18 | ei = zeros(3,1); 19 | ei(i) = 1; 20 | A(:,i) = getOmegaRight(tau,ei,n); 21 | end 22 | dtau = A\w; 23 | end 24 | 25 | function w = getOmegaRight(tau,dtau,n) 26 | [w, R] = getOmegaLeft(tau, dtau, n ); 27 | w = R'*w; 28 | end 29 | 30 | function [w, R] = getOmegaLeft(tau, dtau, n ) 31 | T = -skew(tau); 32 | dT = -skew(dtau); 33 | [sk_w, R] = derivHelper(T,dT,n); 34 | w = skew(sk_w); 35 | end 36 | 37 | function [sk_w,R] = derivHelper(T,dT,n) 38 | A = ( eye(3) - T )^n; 39 | B = ( eye(3) + T )^n; 40 | R = A/B; 41 | 42 | D1 = differ(eye(3)-T,n, -dT); 43 | D2 = R*differ(eye(3)+T,n,dT); 44 | sk_w = (D1 - D2)/A; 45 | end 46 | 47 | function D = differ(A,n, dA) 48 | D = 0*A; 49 | for j = 0:(n-1) 50 | D = D + A^j * dA * A^(n-j-1); 51 | end 52 | end 53 | -------------------------------------------------------------------------------- /v3/orientation_tools/Cayley/cayleyToAngleAxis.m: -------------------------------------------------------------------------------- 1 | function angle_axis = cayleyToAngleAxis(c,n) 2 | 3 | if nargin == 1 4 | n = 1; 5 | end 6 | 7 | angle_axis = rotToAngleAxis( cayleyToRot(c,n) ); 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /v3/orientation_tools/Cayley/cayleyToMRP.m: -------------------------------------------------------------------------------- 1 | function m = cayleyToMRP(c,n) 2 | if nargin == 1 3 | n = 1; 4 | end 5 | 6 | if n ==1 7 | m = c/(1+sqrt(1+c'*c)); 8 | else 9 | m = rotToMRP( calyeyToRot(c,n) ); 10 | end 11 | 12 | end 13 | 14 | -------------------------------------------------------------------------------- /v3/orientation_tools/Cayley/cayleyToQuat.m: -------------------------------------------------------------------------------- 1 | function q = cayleyToQuat(c,n) 2 | 3 | if nargin == 1 4 | n = 1; 5 | end 6 | 7 | if n == 1 8 | q = 1/sqrt(1+c'*c)*[1 ;c]; 9 | else 10 | q = rotToQuat( cayleyToRot(c,n) ); 11 | end 12 | 13 | end 14 | 15 | -------------------------------------------------------------------------------- /v3/orientation_tools/Cayley/cayleyToRot.m: -------------------------------------------------------------------------------- 1 | function R = cayleyToRot(c , n) 2 | 3 | % x = c(1); 4 | % y = c(2); 5 | % z = c(3); 6 | % % See https://en.wikipedia.org/wiki/Rotation_matrix#Skew_parameters_via_Cayley's_formula 7 | % R = 1/(1+x^2+y^2+z^2)*[1+x^2-y^2-z^2 2*x*y-2*z 2*y+2*x*z; ... 8 | % 2*x*y+2*z 1-x^2+y^2-z^2 2*y*z-2*x ; ... 9 | % 2*x*z-2*y 2*x+2*y*z 1-x^2-y^2+z^2]; 10 | 11 | if nargin == 1 12 | n = 1; 13 | end 14 | 15 | if n == 1 16 | cc = c'*c; 17 | R = ( eye(3)*(1-cc)+2*c*c' + 2*skew(c) ) / (1+cc); 18 | else 19 | R = (eye(3)-skew(-c))^n/(eye(3) + skew(-c))^n; 20 | end 21 | 22 | end 23 | 24 | -------------------------------------------------------------------------------- /v3/orientation_tools/Cayley/cayleyToRpy.m: -------------------------------------------------------------------------------- 1 | function rpy = cayleyToRpy(c,n) 2 | 3 | if nargin == 1 4 | n = 1; 5 | end 6 | 7 | rpy = rotToRpy( cayleyToRot(c,n) ); 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpProduct.m: -------------------------------------------------------------------------------- 1 | function [mout] = mrpProduct(m1, m2) 2 | s1 = m1'*m1; 3 | s2 = m2'*m2; 4 | mout = (1-s1)*m2 + (1-s2)*m1 + 2*cross(m1,m2); 5 | mout = mout / (1+s1*s2 - 2*m1'*m2); 6 | end 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpRateLeft.m: -------------------------------------------------------------------------------- 1 | function mDot = mrpRateLeft(m,w) 2 | I = eye(3); 3 | s = m'*m; 4 | mDot = 1/4*[(1-s)*I-2*skew(m)+2*m*m']*w; 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpRateRight.m: -------------------------------------------------------------------------------- 1 | function mDot = mrpRateRight(m,w) 2 | I = eye(3); 3 | s = m'*m; 4 | mDot = 1/4*[(1-s)*I+2*skew(m)+2*m*m']*w; 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpShadow.m: -------------------------------------------------------------------------------- 1 | function ms = mrpShadow(m) 2 | 3 | ms = -m/(m'*m); 4 | 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpToAngleAxis.m: -------------------------------------------------------------------------------- 1 | function angle_axis = mrpToAngleAxis(c) 2 | 3 | angle_axis = rotToAngleAxis( mrpToRot(c) ); 4 | 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpToCayley.m: -------------------------------------------------------------------------------- 1 | function c = mrpToCayley(m) 2 | c = 2*m/(1-m'*m); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpToQuat.m: -------------------------------------------------------------------------------- 1 | function q = mrpToQuat(m) 2 | mm = m'*m; 3 | q = [ (1-mm) ; 2*m ]/(1+mm); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpToRot.m: -------------------------------------------------------------------------------- 1 | function R = mrpToRot(m) 2 | mm = m'*m; 3 | R = eye(3) + (8*skew(m)^2+4*(1-mm)*skew(m))/(1+mm)^2; 4 | end 5 | 6 | -------------------------------------------------------------------------------- /v3/orientation_tools/MRP/mrpToRpy.m: -------------------------------------------------------------------------------- 1 | function rpy = mrpToRpy(m) 2 | 3 | rpy = rotToRpy( mrpToRot(m) ); 4 | 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/SU2toQuat.m: -------------------------------------------------------------------------------- 1 | function q = SU2toQuat(S) 2 | % Converts a special unitary 2x2 matrix to a quaternion 3 | 4 | q = [ real(S(1,1) + S(2,2)) ; 5 | real(S(2,1) - S(1,2)) ; 6 | -imag(S(2,1) + S(1,2)) ; 7 | imag(S(1,1) - S(2,2))]/2; 8 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatConj.m: -------------------------------------------------------------------------------- 1 | function qConj = quatConj(q) 2 | %QUATERN2ROTMAT Converts a quaternion to its conjugate 3 | % 4 | % qConj = quatConj(q) 5 | % 6 | % Converts a quaternion to its conjugate. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | qConj = [q(1) -q(2) -q(3) -q(4)]'; 15 | end 16 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatL.m: -------------------------------------------------------------------------------- 1 | function L = quatL(quat) 2 | % quatL takes a quaternion q1 and returns a 4x4 matrix such that 3 | % quatL(q1)*q2 = q1*q2 (where * means quaternion multiplication) 4 | 5 | 6 | sca = quat(1); 7 | vec = quat(2:4); 8 | L = [sca -vec.' ; vec sca*eye(3)+skew(vec)]; 9 | 10 | end 11 | 12 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatProduct.m: -------------------------------------------------------------------------------- 1 | function [qout] = quatProduct(q1, q2) 2 | r1 = q1(1); v1 = q1(2:4); 3 | r2 = q2(1); v2 = q2(2:4); 4 | 5 | rout = r1*r2 - dot(v1,v2); 6 | vout = r1*v2 +r2*v1 + cross(v1,v2); 7 | qout = [rout ; vout]; 8 | end 9 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatR.m: -------------------------------------------------------------------------------- 1 | function R = quatR(quat) 2 | % quatR takes a quaternion q2 and returns a 4x4 matrix such that 3 | % quatR(q2)*q1 = q1*q2 (where * means quaternion multiplication) 4 | 5 | sca = quat(1); 6 | vec = quat(2:4); 7 | R = [sca -vec.' ; vec sca*eye(3)-skew(vec)]; 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatRateLeft.m: -------------------------------------------------------------------------------- 1 | function quatDot = quatRateLeft(quat,w) 2 | quatDot = quatProduct([0;w]/2, quat); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatRateRight.m: -------------------------------------------------------------------------------- 1 | function quatDot = quatRateRight(quat,w) 2 | quatDot = quatProduct(quat, [0;w]/2); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatToAngleAxis.m: -------------------------------------------------------------------------------- 1 | function [ angle_axis ] = quatToAngleAxis( quat ) 2 | 3 | angle_axis = rotToAngleAxis(quatToRot(quat)); 4 | 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatToCayley.m: -------------------------------------------------------------------------------- 1 | function c = quatToCayley(q) 2 | c = q(2:4)/q(1); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatToMRP.m: -------------------------------------------------------------------------------- 1 | function c = quatToMRP(q) 2 | if q(1) < -.99 3 | q = -q; 4 | end 5 | c = q(2:4)/(1+q(1)); 6 | end 7 | 8 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatToRot.m: -------------------------------------------------------------------------------- 1 | function [ R ] = quatToRot( quat ) 2 | % quatToR takes unit quaternion to rotation matrix 3 | % [R] = quatToR(quat) 4 | % quat= [q0,q1,q2,q3] assumed to follow q0 = cos(angle / 2) while 5 | % [q1,q2,q3] = axis*sin(angle / 2) for angle/axis expression of R 6 | 7 | e0 = quat(1); 8 | e1 = quat(2); 9 | e2 = quat(3); 10 | e3 = quat(4); 11 | 12 | R = [1-2*(e2^2+e3^2) 2*(e1*e2-e0*e3) 2*(e1*e3 + e0*e2) ; 13 | 2*(e1*e2+e0*e3) 1-2*(e1^2+e3^2) 2*(e2*e3 - e0*e1) ; 14 | 2*(e1*e3 -e0*e2) 2*(e2*e3+e0*e1) 1-2*(e1^2+e2^2)]; 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatToRpy.m: -------------------------------------------------------------------------------- 1 | function [ rpy ] = quatToRpy( quat ) 2 | % quatToRpy takes unit quaternion to earth-fixed sequenial roll-pitch-yaw 3 | % euler angles 4 | % [rpy] = quatToRpy(quat) 5 | % quat= [q0,q1,q2,q3] assumed to follow q0 = cos(angle / 2) while 6 | % [q1,q2,q3] = axis*sin(angle / 2) for angle/axis expression of R 7 | % Note: earth-fixed roll-pitch-yaw is the same as body-fixed 8 | % yaw-pitch-roll sequence 9 | 10 | q0 = quat(1); 11 | q1 = quat(2); 12 | q2 = quat(3); 13 | q3 = quat(4); 14 | 15 | rpy = [atan2(2*q2*q3+2*q0*q1,q3^2-q2^2-q1^2+q0^2); 16 | -asin(2*q1*q3-2*q0*q2); 17 | atan2(2*q1*q2+2*q0*q3,q1^2+q0^2-q3^2-q2^2)]; 18 | end 19 | 20 | -------------------------------------------------------------------------------- /v3/orientation_tools/Quaternion/quatToSU2.m: -------------------------------------------------------------------------------- 1 | function S = quatToSU2(q) 2 | % Converts a quaternion to a special unitary 2x2 matrix 3 | 4 | a = q(1); 5 | b = q(2); 6 | c = q(3); 7 | d = q(4); 8 | 9 | i = 1i; 10 | 11 | S = [a+i*d -b-i*c ; b-i*c a-i*d]; 12 | 13 | -------------------------------------------------------------------------------- /v3/orientation_tools/RPY/rpyToAngleAxis.m: -------------------------------------------------------------------------------- 1 | function [ angle_axis ] = rpyToAngleAxis( rpy ) 2 | angle_axis = rotToAngleAxis(rpyToRot(rpy)); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/orientation_tools/RPY/rpyToCayley.m: -------------------------------------------------------------------------------- 1 | function [ cayley ] = rpyToCayley( rpy ) 2 | %UNTITLED5 Summary of this function goes here 3 | % Detailed explanation goes here 4 | cayley = rotToCayley(rpyToRot(rpy)); 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/RPY/rpyToMRP.m: -------------------------------------------------------------------------------- 1 | function [ mrp ] = rpyToMRP( rpy ) 2 | mrp = rotToMRP(rpyToRot(rpy)); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/orientation_tools/RPY/rpyToQuat.m: -------------------------------------------------------------------------------- 1 | function [ quat ] = rpyToQuat( rpy ) 2 | % rpyToQuat takes earth-fixed sequenial roll-pitch-yaw euler angles to a 3 | % unit quaternion 4 | % [quat] = rpyToQuat(rpy) 5 | % quat= [q0,q1,q2,q3] assumed to follow q0 = cos(angle / 2) while 6 | % [q1,q2,q3] = axis*sin(angle / 2) for angle/axis expression of R 7 | % note: earth-fixed roll-pitch-yaw is the same as body-fixed 8 | % yaw-pitch-roll sequence 9 | 10 | roll = rpy(1); 11 | pitch = rpy(2); 12 | yaw = rpy(3); 13 | 14 | halfYaw = yaw *.5; 15 | halfPitch = pitch * .5; 16 | halfRoll = roll *.5; 17 | 18 | cosYaw = cos(halfYaw); 19 | sinYaw = sin(halfYaw); 20 | cosPitch = cos(halfPitch); 21 | sinPitch = sin(halfPitch); 22 | cosRoll = cos(halfRoll); 23 | sinRoll = sin(halfRoll); 24 | 25 | quat = [ cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; 26 | sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; 27 | cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; 28 | cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw]; 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /v3/orientation_tools/RPY/rpyToRot.m: -------------------------------------------------------------------------------- 1 | function [ R ] = rpyToRot( rpy ) 2 | % rpyToR takes earth-fixed sequenial roll-pitch-yaw euler angles to a 3 | % rotation matrix 4 | % [R] = rpyToRot(rpy) 5 | % note: earth-fixed roll-pitch-yaw is the same as body-fixed 6 | % yaw-pitch-roll sequence 7 | 8 | R = Rz(rpy(3))*Ry(rpy(2))*Rx(rpy(1)); 9 | end 10 | 11 | -------------------------------------------------------------------------------- /v3/orientation_tools/Rot/rotToAngleAxis.m: -------------------------------------------------------------------------------- 1 | function aa = rotToAngleAxis(R) 2 | aa = skew(logm(R)); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/orientation_tools/Rot/rotToCayley.m: -------------------------------------------------------------------------------- 1 | function [ cayley ] = rotToCayley( R ) 2 | %UNTITLED5 Summary of this function goes here 3 | % Detailed explanation goes here 4 | % angAx = rotToAngleAxis(R); 5 | % ang = norm(angAx); 6 | % if ang > 0 7 | % ax = angAx / ang; 8 | % else 9 | % ax = [1 0 0]'; 10 | % end 11 | % cayley = [tan(ang/2) * ax]; 12 | 13 | I = eye(3); 14 | cayley = skew((R-I)*inv(I+R)); 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /v3/orientation_tools/Rot/rotToEulerAngles.m: -------------------------------------------------------------------------------- 1 | function [angs1, angs2] = rotToEulerAngles(R,seq) 2 | 3 | % Gets the angle between two vectors 4 | % assumes ax is perpendicular to a0 and a1. ax sets the sign of the result 5 | getAng = @(a0, a1, ax) atan2( dot(ax, cross(a0,a1)) , dot(a0,a1) ); 6 | 7 | I = eye(3); 8 | Re = @(i,th) expm(skew(I(:,i))*th); 9 | 10 | a0 = I(:,seq(1)); 11 | a1 = a0; 12 | b3 = R(:,seq(2)); 13 | c3 = R(:,seq(3)); 14 | c2 = c3; 15 | 16 | angs2 = [0 0 0]'; 17 | for sign = [-1 1] 18 | angs1 = angs2; 19 | b1 = sign*cross(a1,c3); 20 | b1 = b1/norm(b1); 21 | b2 = b1; 22 | b0 = I(:,seq(2)); 23 | angs2(1) = getAng(b0, b1, a0); 24 | 25 | c1 = Re(seq(1), angs2(1)); 26 | c1 = c1(:,seq(3)); 27 | angs2(2) = getAng(c1, c2, b1); 28 | angs2(3) = getAng(b2, b3, c3); 29 | end 30 | 31 | 32 | end 33 | 34 | -------------------------------------------------------------------------------- /v3/orientation_tools/Rot/rotToFixedAngles.m: -------------------------------------------------------------------------------- 1 | function [angs1, angs2] = rotToFixedAngles(R,seq) 2 | 3 | [angs1,angs2] = rotToEulerAngles(R, seq([3 2 1])); 4 | angs1 = angs1([3 2 1]); 5 | angs2 = angs2([3 2 1]); 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /v3/orientation_tools/Rot/rotToMRP.m: -------------------------------------------------------------------------------- 1 | function [ mrp ] = rotToMRP( R ) 2 | %UNTITLED5 Summary of this function goes here 3 | % Detailed explanation goes here 4 | angAx = rotToAngleAxis(R); 5 | [ang ax] = angleAxisToAngleAndAxis(angAx); 6 | mrp = [tan(ang/4) * ax]; 7 | end 8 | 9 | -------------------------------------------------------------------------------- /v3/orientation_tools/Rot/rotToQuat.m: -------------------------------------------------------------------------------- 1 | function [ quat ] = rotToQuat( R ) 2 | %UNTITLED5 Summary of this function goes here 3 | % Detailed explanation goes here 4 | angAx = rotToAngleAxis(R); 5 | quat = angleAxisToQuat(angAx); 6 | end 7 | 8 | -------------------------------------------------------------------------------- /v3/orientation_tools/Rot/rotToRpy.m: -------------------------------------------------------------------------------- 1 | function [rpy1 rpy2] = rotToRpy(R) 2 | 3 | [rpy1 rpy2] = rotToFixedAngles(R,[1 2 3]); 4 | 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/orientation_tools/Util/Rx.m: -------------------------------------------------------------------------------- 1 | function R = Rx( theta ) 2 | 3 | % rx 3x3 coordinate rotation (X-axis) 4 | % rx(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common X axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | R = [ 1 0 0; 12 | 0 c -s; 13 | 0 s c ]; 14 | -------------------------------------------------------------------------------- /v3/orientation_tools/Util/Ry.m: -------------------------------------------------------------------------------- 1 | function R = Ry( theta ) 2 | 3 | % ry 3x3 coordinate rotation (Y-axis) 4 | % ry(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common Y axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | R = [ c 0 s; 12 | 0 1 0; 13 | -s 0 c ]; 14 | -------------------------------------------------------------------------------- /v3/orientation_tools/Util/Rz.m: -------------------------------------------------------------------------------- 1 | function R = Rz( theta ) 2 | 3 | % rz 3x3 coordinate rotation (Z-axis) 4 | % rz(theta) calculates the 3x3 rotational coordinate transform matrix from 5 | % A to B coordinates, where coordinate frame B is rotated by an angle theta 6 | % (radians) relative to frame A about their common Z axis. 7 | 8 | c = cos(theta); 9 | s = sin(theta); 10 | 11 | R = [ c -s 0; 12 | s c 0; 13 | 0 0 1 ]; 14 | -------------------------------------------------------------------------------- /v3/orientation_tools/Util/isEqual.m: -------------------------------------------------------------------------------- 1 | function flag = isEqual(a,b,tol) 2 | 3 | if nargin == 2 4 | tol = sqrt(eps)/2; 5 | end 6 | d = a-b; 7 | flag = max(abs(d(:)))<=tol; 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /v3/orientation_tools/Util/isEqualModSign.m: -------------------------------------------------------------------------------- 1 | function flag = isEqualModSign(a,b,tol) 2 | 3 | if nargin == 2 4 | tol = sqrt(eps)/2; 5 | end 6 | 7 | flag = isEqual(a,b,tol) | isEqual(a,-b,tol); 8 | end 9 | 10 | -------------------------------------------------------------------------------- /v3/orientation_tools/Util/skew.m: -------------------------------------------------------------------------------- 1 | function out = skew( in ) 2 | 3 | % skew convert 3D vector <--> 3x3 skew-symmetric matrix 4 | % S=skew(v) and v=skew(A) calculate the 3x3 skew-symmetric matrix S 5 | % corresponding to the given 3D vector v, and the 3D vector corresponding 6 | % to the skew-symmetric component of the given arbitrary 3x3 matrix A. For 7 | % vectors a and b, skew(a)*b is the cross product of a and b. If the 8 | % argument is a 3x3 matrix then it is assumed to be A, otherwise it is 9 | % assumed to be v. skew(A) produces a column-vector result, but skew(v) 10 | % will accept a row or column vector argument. 11 | 12 | if all(size(in)==[3 3]) % do v = skew(A) 13 | out = 0.5 * [ in(3,2) - in(2,3); 14 | in(1,3) - in(3,1); 15 | in(2,1) - in(1,2) ]; 16 | else % do S = skew(v) 17 | out = [ 0, -in(3), in(2); 18 | in(3), 0, -in(1); 19 | -in(2), in(1), 0 ]; 20 | end 21 | -------------------------------------------------------------------------------- /v3/regressor/RegressorClassical.m: -------------------------------------------------------------------------------- 1 | function [Y, Y_rot] = RegressorClassical( model, q, qd,qdd) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) 7 | [q,qd,qdd] = confVecToCell(model,q,qd,qdd); 8 | 9 | end 10 | 11 | Y = zeros(model.NV, model.NB*10); 12 | Y_rot = zeros(model.NV, model.NB_rot*10); 13 | 14 | p = model.parent; 15 | a_grav = get_gravity(model); 16 | for i = 1:model.NB 17 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 18 | vJ = S{i}*qd{i}; 19 | Xup{i} = XJ * model.Xtree{i}; 20 | if p(i) == 0 21 | v{i} = vJ; 22 | a{i}= -Xup{i}*a_grav + S{i} * qdd{i} + crm(v{i})*vJ; 23 | else 24 | v{i} = Xup{i}*v{p(i)} + vJ; 25 | a{i}= Xup{i}*a{p(i)} + S{i} * qdd{i} + crm(v{i})*vJ; 26 | end 27 | F{i} = individualRegressor(a{i}, v{i}); 28 | 29 | if model.has_rotor(i) 30 | [ XJ_rotor, S_rotor{i} ] = jcalc( model.jtype_rotor{i}, q{i}*model.gr{i} ); 31 | S_rotor{i} = S_rotor{i} * model.gr{i}; 32 | vJ_rotor = S_rotor{i} * qd{i}; 33 | Xup_rotor{i} = XJ_rotor * model.Xrotor{i}; 34 | if model.parent(i) == 0 35 | v_rotor{i} = vJ_rotor; 36 | a_rotor{i} = Xup_rotor{i}*(-a_grav) + S_rotor{i}*qdd{i}; 37 | else 38 | v_rotor{i} = Xup_rotor{i}*v{model.parent(i)} + vJ_rotor; 39 | a_rotor{i} = Xup_rotor{i}*a{model.parent(i)} + S_rotor{i}*qdd{i} + crm(v_rotor{i})*vJ_rotor; 40 | end 41 | F_rotor{i} = individualRegressor(a_rotor{i}, v_rotor{i}); 42 | end 43 | end 44 | 45 | 46 | 47 | for i = model.NB:-1:1 48 | ii = model.vinds{i}; 49 | param_inds = 10*(i-1)+1 : 10*i; 50 | Y(ii,param_inds) = S{i}' * F{i}; 51 | Fi = Xup{i}' * F{i}; 52 | j = i; 53 | j = model.parent(j); 54 | while j ~= 0 55 | jj = model.vinds{j}; 56 | Y(jj,param_inds) = S{j}' * Fi; 57 | Fi = Xup{j}' * Fi; 58 | j = model.parent(j); 59 | end 60 | 61 | if model.has_rotor(i) 62 | param_inds = model.rotor_param_inds{i}; 63 | 64 | Y_rot(ii,param_inds) = S_rotor{i}' * F_rotor{i}; 65 | Fi = Xup_rotor{i}' * F_rotor{i}; 66 | j = i; 67 | j = model.parent(j); 68 | while j ~= 0 69 | jj = model.vinds{j}; 70 | Y_rot(jj,param_inds) = S{j}' * Fi; 71 | Fi = Xup{j}' * Fi; 72 | j = model.parent(j); 73 | end 74 | end 75 | end 76 | 77 | -------------------------------------------------------------------------------- /v3/regressor/RegressorHandG.m: -------------------------------------------------------------------------------- 1 | function [YH, Yg, YH_rotor, Yg_rot] = RegressorHandG(model, q) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | 7 | YH = []; 8 | Yg = []; 9 | YH_rotor = []; 10 | Yg_rot = []; 11 | 12 | qd = zeros(model.NV,1); 13 | qdd = zeros(model.NV,1); 14 | 15 | [Yg, Yg_rot] = RegressorClassical(model,q,qd,qdd); 16 | model.gravity = [0 0 0]'; 17 | 18 | for i = 1:model.NV 19 | qdd(i) = 1; 20 | [Y, Y_rotor] = RegressorClassical(model,q,qd,qdd); 21 | qdd(i) = 0; 22 | YH = [ YH ; Y]; 23 | if sum(model.NB_rot) > 0 24 | YH_rotor = [YH_rotor ; Y_rotor]; 25 | end 26 | end 27 | -------------------------------------------------------------------------------- /v3/regressor/Regressor_HqdandCTqd.m: -------------------------------------------------------------------------------- 1 | function [Y_Hqd, Y_CTqd, Y_Hqd_rot, Y_CTqd_rot] = Regressor_HqdandCTqd( model, q , qd) 2 | % Indirect regressors term YHqd a = H qd and YCTqd = C^T qd 3 | 4 | if ~isfield(model,'nq') 5 | model = postProcessModel(model); 6 | end 7 | if ~iscell(q) 8 | [q, qd] = confVecToCell(model,q,qd); 9 | end 10 | 11 | Y_Hqd = zeros(model.NV, 10*model.NB); 12 | Y_Hqd_rot = zeros(model.NV,10*model.NB_rot); 13 | 14 | Y_CTqd = zeros(model.NV, 10*model.NB); 15 | Y_CTqd_rot = zeros(model.NV,10*model.NB_rot); 16 | 17 | for i = 1:model.NB 18 | [ XJ, S{i} ] = jcalc( model.jtype{i}, q{i} ); 19 | vJ = S{i}*qd{i}; 20 | Xup{i} = XJ * model.Xtree{i}; 21 | if model.parent(i) == 0 22 | v{i} = vJ; 23 | else 24 | v{i} = Xup{i}*v{model.parent(i)} + vJ; 25 | end 26 | Sd{i} = crm(v{i})*S{i}; 27 | hi = individualRegressor(v{i}, 0*v{i}); 28 | param_inds = 10*(i-1) + (1:10); 29 | j = i; 30 | while j > 0 31 | jj = model.vinds{j}; 32 | Y_Hqd(jj, param_inds) = S{j}' * hi; 33 | Y_CTqd(jj, param_inds)= Sd{j}'* hi; 34 | hi = Xup{j}' * hi; 35 | j = model.parent(j); 36 | end 37 | 38 | if model.has_rotor(i) 39 | 40 | [ XJ_rotor, S_rotor{i} ] = jcalc( model.jtype_rotor{i}, q{i}*model.gr{i} ); 41 | S_rotor{i} = S_rotor{i} * model.gr{i}; 42 | vJ_rotor = S_rotor{i} * qd{i}; 43 | Xup_rotor{i} = XJ_rotor * model.Xrotor{i}; 44 | if model.parent(i) == 0 45 | v_rotor{i} = vJ_rotor; 46 | else 47 | v_rotor{i} = Xup_rotor{i}*v{model.parent(i)} + vJ_rotor; 48 | end 49 | Sd_rotor{i} = crm(v_rotor{i})*S_rotor{i}; 50 | 51 | hi_rotor = individualRegressor(v_rotor{i}, v_rotor{i}*0); 52 | 53 | param_inds = model.rotor_param_inds{i}; 54 | ii = model.vinds{i}; 55 | 56 | Y_Hqd_rot(ii, param_inds) = S_rotor{i}' * hi_rotor; 57 | Y_CTqd_rot(ii,param_inds) = Sd_rotor{i}'* hi_rotor; 58 | 59 | hi_rotor = Xup_rotor{i}'*hi_rotor; 60 | j = model.parent(i); 61 | 62 | while j > 0 63 | jj = model.vinds{j}; 64 | Y_Hqd_rot(jj, param_inds) = S{j}' * hi_rotor; 65 | Y_CTqd_rot(jj,param_inds) = Sd{j}'* hi_rotor; 66 | hi_rotor = Xup{j}' * hi_rotor; 67 | j = model.parent(j); 68 | end 69 | end 70 | end 71 | 72 | -------------------------------------------------------------------------------- /v3/regressor/individualRegressor.m: -------------------------------------------------------------------------------- 1 | function Y = individualRegressor(a,v) 2 | %INDIVIDUALREGRESSOR 3 | % OUT1 = INDIVIDUALREGRESSOR(IN1,IN2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.4. 6 | % 04-Dec-2020 23:47:52 7 | 8 | a1 = a(1,:); 9 | a2 = a(2,:); 10 | a3 = a(3,:); 11 | a4 = a(4,:); 12 | a5 = a(5,:); 13 | a6 = a(6,:); 14 | v1 = v(1,:); 15 | v2 = v(2,:); 16 | v3 = v(3,:); 17 | v4 = v(4,:); 18 | v5 = v(5,:); 19 | v6 = v(6,:); 20 | t2 = v1.*v2; 21 | t3 = v1.*v3; 22 | t4 = v2.*v3; 23 | t5 = v1.*v5; 24 | t6 = v2.*v4; 25 | t7 = v1.*v6; 26 | t8 = v3.*v4; 27 | t9 = v2.*v6; 28 | t10 = v3.*v5; 29 | t11 = v1.^2; 30 | t12 = v2.^2; 31 | t13 = v3.^2; 32 | t14 = a1+t4; 33 | t15 = a2+t3; 34 | t16 = a3+t2; 35 | t17 = -t2; 36 | t18 = -t3; 37 | t19 = -t4; 38 | t20 = -t6; 39 | t21 = -t7; 40 | t22 = -t10; 41 | t23 = -t11; 42 | t24 = -t12; 43 | t25 = -t13; 44 | t26 = a6+t5+t20; 45 | t27 = a5+t8+t21; 46 | t28 = a4+t9+t22; 47 | Y = reshape([0.0,0.0,0.0,t28,t27,t26,0.0,-a6-t5+t6,t27,t24+t25,t16,-a2+t3,t26,0.0,-a4-t9+t10,-a3+t2,t23+t25,t14,-a5+t7-t8,t28,0.0,t15,-a1+t4,t23+t24,a1,t3,t17,0.0,0.0,0.0,t19,a2,t2,0.0,0.0,0.0,t4,t18,a3,0.0,0.0,0.0,t12+t25,a3+t17,t15,0.0,0.0,0.0,t16,t13+t23,a1+t19,0.0,0.0,0.0,a2+t18,t14,t11+t24,0.0,0.0,0.0],[6,10]); 48 | -------------------------------------------------------------------------------- /v3/spatial/AdjointToSE3.m: -------------------------------------------------------------------------------- 1 | function T = AdjointToSE3(X) 2 | R = X(1:3,1:3); 3 | p = skew( X(4:6,1:3)*R' ); 4 | T = [R p; 0 0 0 1]; 5 | end 6 | -------------------------------------------------------------------------------- /v3/spatial/SE3toAdjoint.m: -------------------------------------------------------------------------------- 1 | function X = SE3toAdjoint(T) 2 | R = T(1:3,1:3); 3 | p = T(1:3,4); 4 | O = zeros(3); 5 | X = [R O; skew(p)*R R]; 6 | end 7 | -------------------------------------------------------------------------------- /v3/spatial/crmExtract.m: -------------------------------------------------------------------------------- 1 | function v = crmExtract( vcross ) 2 | 3 | v = zeros(6,1); 4 | v(1:3) = skew(vcross(1:3,1:3)); 5 | v(4:6) = skew(vcross(4:6,1:3)); 6 | -------------------------------------------------------------------------------- /v3/spatial/factorFunctions.m: -------------------------------------------------------------------------------- 1 | function B = factorFunctions(I, v, number) 2 | if nargin == 2 3 | number = 3; 4 | end 5 | if number == 1 6 | B = crf(v)*I; 7 | elseif number == 2 8 | B = icrf( I * v) - I * crm(v); 9 | else 10 | B = 1/2 * (crf(v)*I + icrf( I * v) - I * crm(v)); 11 | end 12 | end -------------------------------------------------------------------------------- /v3/spatial/icrf.m: -------------------------------------------------------------------------------- 1 | function mat = icrf(f) 2 | 3 | mat = [-skew(f(1:3)) -skew(f(4:6));-skew(f(4:6)) zeros(3)]; 4 | 5 | -------------------------------------------------------------------------------- /v3/unit_tests/UnitTest_All.m: -------------------------------------------------------------------------------- 1 | printHeader('CMM'); 2 | UnitTest_CMM 3 | 4 | printHeader('Coriolis'); 5 | UnitTest_Coriolis 6 | 7 | printHeader('Derivatives'); 8 | UnitTest_Derivatives 9 | 10 | printHeader('Derivatives (Floating)'); 11 | UnitTest_Derivatives_FB 12 | 13 | printHeader('Main Dynamics'); 14 | UnitTest_Dynamics 15 | 16 | printHeader('Orientation'); 17 | UnitTest_Orientation 18 | 19 | printHeader('Orientation Rates'); 20 | UnitTest_OrientationRates 21 | 22 | function printHeader(st) 23 | fprintf('************************************\n'); 24 | fprintf('%s\n',st); 25 | fprintf('************************************\n'); 26 | end -------------------------------------------------------------------------------- /v3/unit_tests/UnitTest_CMM.m: -------------------------------------------------------------------------------- 1 | clear 2 | N = 3; 3 | 4 | % Create a random model with N links 5 | model = autoTree(N, 1, pi/3); 6 | model.jtype{1} = 'Fb'; 7 | model = postProcessModel(model); 8 | 9 | % Random inertial properties 10 | for i = 1:model.NB 11 | model.I{i} = inertiaVecToMat( rand(10,1) ); 12 | model.I_rotor{i} = inertiaVecToMat( rand(10,1) ); 13 | end 14 | [a, a_rot] = getModelInertialParams(model); 15 | 16 | % Random configuration and velocity 17 | q = rand(model.NQ,1); 18 | q = normalizeConfVec(model, q); 19 | qd = rand(model.NV,1); 20 | 21 | % Compute CMM with CMM algo 22 | [Ag, Agd_qd] = CMM(model,q, qd); 23 | [Ag2] = CMM_from_CRBA(model,q); 24 | 25 | % Total energy and momentum from EnerMo 26 | ret = EnerMo( model, q, qd ); 27 | p0G = ret.cm; 28 | X0G = [eye(3) zeros(3); skew(p0G) eye(3)]; 29 | hG = X0G'*ret.htot; 30 | 31 | checkValue('Centroidal Momentum', hG, Ag2*qd) 32 | checkValue('Centroidal Mom Matrix', Ag, Ag2) 33 | 34 | dt = sqrt(eps)*1i; 35 | q_new = configurationAddition(model,q,dt*qd); 36 | Ag_new = CMM(model,q_new); 37 | Agdot = CMMTimeDerivative(model, q, qd); 38 | Agdot_finite_difference = real( (Ag_new-Ag) / dt ); 39 | 40 | checkValue('Centroidal Mom Matrix Time Derivative', Agdot, Agdot_finite_difference) 41 | checkValue('Agdot_qdot', Agd_qd, Agdot*qd) 42 | 43 | 44 | function checkValue(name, v1, v2, tolerance) 45 | if nargin == 3 46 | tolerance = sqrt(eps); 47 | end 48 | value = norm(v1(:)-v2(:)); 49 | fprintf('%s \t %e\n',name,value); 50 | if value > tolerance 51 | error('%s is out of tolerance',name); 52 | end 53 | end 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /v3/unit_tests/UnitTest_Coriolis.m: -------------------------------------------------------------------------------- 1 | % Sanity Check Example for Coriolis Matrix Algorithm 2 | 3 | for N = [10] 4 | % Create a random model with N links 5 | model = autoTree(N, 1, pi/3); 6 | model = postProcessModel(model); 7 | checkDynamics(model,sprintf('N=%d',N)); 8 | end 9 | 10 | function checkDynamics(model, desc) 11 | fprintf('====================================\n'); 12 | fprintf('%s\n',desc); 13 | fprintf('====================================\n'); 14 | model.gravity = [0 0 0]'; 15 | 16 | eCqd = 0; 17 | eHdot = 0; 18 | eGamma = 0; 19 | 20 | for i = 1:100 21 | % Random inertial properties 22 | for i = 1:model.NB 23 | model.I{i} = inertiaVecToMat( rand(10,1) ); 24 | end 25 | 26 | % Random configuration and velocity 27 | q = rand(model.NQ,1)*2*pi; 28 | q = normalizeConfVec(model, q); 29 | 30 | qd = rand(model.NV,1)*10; 31 | qdd = zeros(model.NV,1); 32 | 33 | % Calculate dynamics quanitites 34 | Cqd = ID(model, q ,qd ,qdd); % Inverse dynamics 35 | [C,Hdot] = CoriolisMatrix( model, q, qd); % Coriolis matrix 36 | 37 | eCqd = max(eCqd , norm(Cqd-C*qd,Inf) ); 38 | eHdot= max(eHdot , norm(Hdot-C-C',Inf) ); 39 | 40 | % Check Christoffel 41 | if ~any(model.nv > 1) && ~any(model.has_rotor) 42 | Gamma = Christoffel(model,q); 43 | C2 = 0*C; 44 | for i = 1:model.NB 45 | C2 = C2 + Gamma(:,:,i)*qd(i); 46 | end 47 | eGamma = max(eGamma,norm(C2-C,Inf)); 48 | end 49 | end 50 | 51 | checkValue('Hdot=C+CT' , eHdot , 0 ); % Generalized Coriolis force 52 | checkValue('C-Gamma*qd' , eGamma , 0 ); % Hdot -2C skew symmetric 53 | checkValue('Cqd' , eCqd , 0 ); % Christoffel 54 | 55 | 56 | fprintf('\n'); 57 | end 58 | 59 | function checkValue(name, v1, v2, tolerance) 60 | if nargin == 3 61 | tolerance = sqrt(eps); 62 | end 63 | value = norm(v1(:)-v2(:)); 64 | fprintf('%s \t %e\n',name,value); 65 | if value > tolerance 66 | error('%s is out of tolerance',name); 67 | end 68 | end -------------------------------------------------------------------------------- /v3/unit_tests/UnitTest_Identifiability_Basis_via_load.m: -------------------------------------------------------------------------------- 1 | % This is intended to verify the matrices (Minimal_Basis and Perp_Basis) obtained from 2 | % "RPNA_URDF_Examples.m". 3 | clear; 4 | clc; 5 | 6 | RPNA_Examples() 7 | save('RPNA_Examples_Results.mat','Perp_Basis','Minimal_Basis'); 8 | 9 | clear; 10 | clc; 11 | RPNA_URDF_Example() 12 | save('RPNA_URDF_Example_Results.mat','Perp_Basis','Minimal_Basis'); 13 | 14 | clear; 15 | clc; 16 | %% 17 | [model_URDF, robotics_toolbox_robot] = URDF_to_spatialv2_model("panda_arm_no_fixed.urdf"); 18 | 19 | dof = robotics_toolbox_robot.NumBodies; 20 | W = cell(dof,1); 21 | for i = 1:dof 22 | Ixx = robotics_toolbox_robot.Bodies{1,i}.Inertia(1); 23 | Iyy = robotics_toolbox_robot.Bodies{1,i}.Inertia(2); 24 | Izz = robotics_toolbox_robot.Bodies{1,i}.Inertia(3); 25 | Iyz = robotics_toolbox_robot.Bodies{1,i}.Inertia(4); 26 | Ixz = robotics_toolbox_robot.Bodies{1,i}.Inertia(5); 27 | Ixy = robotics_toolbox_robot.Bodies{1,i}.Inertia(6); 28 | m = robotics_toolbox_robot.Bodies{1,i}.Mass; 29 | Cx = robotics_toolbox_robot.Bodies{1,i}.CenterOfMass(1); 30 | Cy = robotics_toolbox_robot.Bodies{1,i}.CenterOfMass(2); 31 | Cz = robotics_toolbox_robot.Bodies{1,i}.CenterOfMass(3); 32 | W{i,1} = [ m , m*Cx, m*Cy, m*Cz, Ixx, Iyy, Izz, Iyz, Ixz, Ixy]'; 33 | end 34 | W = cell2mat(W); 35 | 36 | model_DH = Panda_model(W); 37 | robotics_toolbox_robot.DataFormat = 'column'; 38 | robotics_toolbox_robot.Gravity = [0 0 -9.81]'; 39 | %% load basis results of Panda robot modeled by DH or URDF 40 | 41 | % panda basis from DH 42 | load RPNA_Examples_Results.mat 43 | Perp_Basis_DH = Perp_Basis; 44 | Minimal_Basis_DH = Minimal_Basis; 45 | 46 | % panda basis from URDF 47 | load RPNA_URDF_Example_Results.mat 48 | Perp_Basis_URDF = Perp_Basis; 49 | Minimal_Basis_URDF = Minimal_Basis; 50 | 51 | %% Verification loop 52 | timeDuration = 2.5; 53 | currentTime = 0; 54 | timeStep = 0.005; 55 | for i =0:timeDuration/timeStep 56 | q = -cos(currentTime)*ones(dof,1); 57 | qd = sin(currentTime)*ones(dof,1); 58 | qdd = cos(currentTime)*ones(dof,1); 59 | qd_r = qd; 60 | disp("===============================================") 61 | 62 | tau1 = inverseDynamics(robotics_toolbox_robot, q, qd, qdd); 63 | 64 | [tau2, ~] = ID( model_DH, q, qd, qdd ); 65 | checkValue('ID with model from DH',tau2, tau1, 1e-5); 66 | 67 | [tau3, ~] = ID( model_URDF, q, qd, qdd ); 68 | checkValue('ID with model from URDF',tau3, tau1, 1e-5); 69 | 70 | [Y_DH, ~] = RegressorClassical( model_DH, q, qd,qdd); 71 | tau4 = Y_DH*W; 72 | checkValue('Torque of model from DH',tau4, tau1, 1e-5); 73 | 74 | tau5 = Y_DH * Minimal_Basis_DH * Perp_Basis_DH' *W; 75 | checkValue('Torque of model from DH with basis',tau5, tau1, 1e-5); 76 | 77 | [Y_URDF, ~] = RegressorClassical( model_URDF, q, qd,qdd); 78 | tau6 = Y_URDF*W; 79 | checkValue('Torque of model from URDF',tau6, tau1, 1e-5); 80 | 81 | tau7 = Y_URDF * Minimal_Basis_URDF * Perp_Basis_URDF' *W; 82 | checkValue('Torque of model from URDF with basis',tau7, tau1, 1e-5); 83 | 84 | currentTime = currentTime + timeStep; 85 | end 86 | 87 | function checkValue(name, v1, v2, tolerance) 88 | if nargin == 3 89 | tolerance = sqrt(eps); 90 | end 91 | value = norm(v1(:)-v2(:)); 92 | fprintf('%s \t %e\n',name,value); 93 | if value > tolerance 94 | error('%s is out of tolerance',name); 95 | end 96 | end -------------------------------------------------------------------------------- /v3/unit_tests/UnitTest_OrientationRates.m: -------------------------------------------------------------------------------- 1 | clear; 2 | c = rand(3,1); 3 | R = cayleyToRot(c); 4 | wR = rand(3,1); 5 | wL = R*wR; 6 | mrp = rotToMRP(R); 7 | a = rotToAngleAxis(R); 8 | q = rotToQuat(R); 9 | 10 | 11 | dt = sqrt(eps); 12 | Rnew = R*angleAxisToRot(wR*dt); 13 | 14 | %% Check Quat Rates 15 | qnew = rotToQuat(Rnew); 16 | qdot_numer = (qnew-q)/dt; 17 | qdot = quatRateRight(q,wR); 18 | qdot2 = quatRateLeft(q,wL); 19 | 20 | assert( isEqual(qdot, qdot_numer, 1e-6 ) ) 21 | assert( isEqual(qdot2, qdot_numer, 1e-6 ) ) 22 | 23 | %% Check Cayley Rates 24 | cnew = rotToCayley(Rnew); 25 | cdot_numer = (cnew-c)/dt; 26 | 27 | cdot = cayleyRateRight(c,wR); 28 | cdot2 = cayleyRateLeft(c,wL); 29 | 30 | assert( isEqual(cdot, cdot_numer, 1e-6 ) ); 31 | assert( isEqual(cdot2, cdot_numer, 1e-6 ) ); 32 | 33 | %% Check MRP Rates 34 | mnew = rotToMRP(Rnew); 35 | mdot_numer = (mnew - mrp)/dt; 36 | 37 | mdot = mrpRateRight(mrp, wR); 38 | mdot2 = mrpRateLeft(mrp, wL); 39 | 40 | assert( isEqual(mdot, mdot_numer, 1e-6 ) ) 41 | assert( isEqual(mdot2, mdot_numer, 1e-6 ) ) 42 | 43 | %% Check Angle Axis Rates 44 | anew = rotToAngleAxis(Rnew); 45 | adot_numer = (anew-a)/dt; 46 | 47 | adot = angleAxisRateRight(a, wR); 48 | adot2 = angleAxisRateLeft(a, wL); 49 | 50 | assert( isEqual(adot, adot_numer, 1e-6 ) ) 51 | assert( isEqual(adot2, adot_numer, 1e-6 ) ) 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /v3/unit_tests/UnitTest_URDF_import.m: -------------------------------------------------------------------------------- 1 | [spatial_v2_model, robotics_toolbox_robot] = URDF_to_spatialv2_model('puma560_robot.urdf', 1); 2 | q = rand(6,1); 3 | qd = rand(6,1); 4 | qdd = rand(6,1); 5 | 6 | robotics_toolbox_robot.DataFormat = 'column'; 7 | robotics_toolbox_robot.Gravity = [0 0 -9.81]'; 8 | 9 | 10 | M1 = massMatrix(robotics_toolbox_robot, q); 11 | [H, ~] = HandC(spatial_v2_model, q,0*q); 12 | 13 | tau1 = inverseDynamics(robotics_toolbox_robot, q, qd, qdd); 14 | tau2 = ID(spatial_v2_model, q, qd, qdd); 15 | 16 | checkValue('MassMatrix',M1, H, 1e-5); 17 | checkValue('InverseDyn',tau1,tau2, 1e-5); 18 | 19 | 20 | function checkValue(name, v1, v2, tolerance) 21 | if nargin == 3 22 | tolerance = sqrt(eps); 23 | end 24 | value = norm(v1(:)-v2(:)); 25 | fprintf('%s \t %e\n',name,value); 26 | if value > tolerance 27 | error('%s is out of tolerance',name); 28 | end 29 | end 30 | -------------------------------------------------------------------------------- /v3/utility/affineInvariantDistance.m: -------------------------------------------------------------------------------- 1 | function d = affineInvariantDistance( P, Q ) 2 | d = norm( 1/sqrt(2) * log( eig(P\Q) ) ); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/utility/configuration/confVecToCell.m: -------------------------------------------------------------------------------- 1 | function [q_cell, v_cell, vd_cell, vd2_cell] = confVecToCell(model,q,v,vd, vd2) 2 | 3 | qj = 0; 4 | vj = 0; 5 | 6 | if ~isfield(model,'nq') 7 | model.nq = ones(model.NB,1); 8 | model.nv = ones(model.NB,1); 9 | end 10 | 11 | q_cell = cell(model.NB,size(q,2)); 12 | if nargin > 2 13 | v_cell = cell(model.NB,size(v,2)); 14 | end 15 | if nargin > 3 16 | vd_cell = cell(model.NB,size(vd,2)); 17 | end 18 | if nargin > 4 19 | vd2_cell = cell(model.NB,size(vd2,2)); 20 | end 21 | 22 | for i = 1:model.NB 23 | 24 | q_cell{i} = q(qj+1 : qj+model.nq(i),: ); 25 | if nargin > 2 26 | v_cell{i} = v( vj+1 : vj+model.nv(i),: ); 27 | end 28 | if nargin > 3 29 | vd_cell{i} = vd( vj+1 : vj+model.nv(i),: ); 30 | end 31 | if nargin > 4 32 | vd2_cell{i} = vd2( vj+1 : vj+model.nv(i),: ); 33 | end 34 | 35 | qj = qj+model.nq(i); 36 | vj = vj+model.nv(i); 37 | end 38 | 39 | end -------------------------------------------------------------------------------- /v3/utility/configuration/configurationAddition.m: -------------------------------------------------------------------------------- 1 | function new_q = configurationAddition(model,q,dq) 2 | 3 | USE_MCX = 0; 4 | 5 | % assert(USE_MCX == 0 , 'MCX Not Yet Supported'); 6 | % assert(1==2,'Not Yet Implemented') 7 | if ~isfield(model,'nq') 8 | model = postProcessModel(model); 9 | end 10 | if ~iscell(q) 11 | [q, dq] = confVecToCell(model,q,dq); 12 | end 13 | 14 | if USE_MCX 15 | new_q = MultiComplex.zeros(model.NQ,1); 16 | else 17 | new_q = q{1}(1)*0 + zeros(model.NQ,1); 18 | end 19 | 20 | for i = 1:model.NB 21 | ii = model.qinds{i}; 22 | 23 | switch model.jtype{i} 24 | case {'Fb'} 25 | X = jcalc('Fb',q{i}); 26 | v = dq{i}(4:6); 27 | Rup = X(1:3,1:3); 28 | 29 | % tangent element 30 | tang = quatR([0 ; dq{i}(1:3)/2 ]); 31 | 32 | if ~isreal(dq{i}) 33 | if USE_MCX 34 | qt_new = expmComplexStep(tang)* q{i}(1:4); 35 | else 36 | qt_new = (eye(4) + tang)* q{i}(1:4); 37 | end 38 | else 39 | qt_new = expm(tang)* q{i}(1:4); 40 | end 41 | 42 | p_new = q{i}(5:7) + Rup.'*v; 43 | new_q(ii) = [qt_new ; p_new]; 44 | 45 | case {'S'} 46 | 47 | tang = quatR([0 ; dq{i}/2 ]); 48 | if ~isreal(dq{i}) 49 | if USE_MCX 50 | new_q(ii) = expmComplexStep(tang)* q{i}; 51 | else 52 | new_q(ii) = (eye(4) + tang)*q{i}; 53 | end 54 | else 55 | new_q(ii) = expm(tang)* q{i}; 56 | end 57 | case {'SO3'} 58 | R = reshape(q{i},[3 3]); 59 | so3 = -skew(dq{i}); 60 | if ~isreal(dq{i}) 61 | if USE_MCX 62 | new_q(ii) = reshape( expmComplexStep(so3)* R, [9 1]); 63 | else 64 | new_q(ii) = reshape( (eye(3) + so3)* R, [9 1]); 65 | end 66 | else 67 | new_q(ii) = reshape(expm(so3) * R, [9 1]); 68 | end 69 | case {'SE3'} 70 | Tup = reshape(q{i},[4 4]); 71 | se3 = -vecTose3(dq{i}); 72 | if ~isreal(dq{i}) 73 | if USE_MCX 74 | new_q(ii) = reshape( expmComplexStep(se3)*Tup, [16 1]); 75 | else 76 | new_q(ii) = reshape( (eye(4) + se3)*Tup, [16 1]); 77 | end 78 | else 79 | new_q(ii) = reshape( expm(se3)*Tup, [16 1]); 80 | end 81 | otherwise 82 | new_q(ii) = q{i} + dq{i}; 83 | end 84 | end 85 | if isreal(new_q) 86 | new_q = real(new_q); 87 | else if USE_MCX && new_q.order() == 1 88 | new_q = real(new_q) + imag(new_q)*1i; 89 | end 90 | end 91 | -------------------------------------------------------------------------------- /v3/utility/configuration/configurationRates.m: -------------------------------------------------------------------------------- 1 | function dot_q = configurationRates(model,q,qd) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) 7 | [q, qd] = confVecToCell(model,q,qd); 8 | end 9 | 10 | dot_q = zeros(model.NQ,1); 11 | 12 | for i = 1:model.NB 13 | ii = model.qinds{i}; 14 | 15 | switch model.jtype{i} 16 | case {'Fb'} 17 | Rup = rq(q{i}(1:4)); 18 | dot_q(ii) = [rqd(q{i}(1:4),qd{i}(1:3) ) ; 19 | Rup' * qd{i}(4:6)]; 20 | case {'S'} 21 | Rup = rq(q{i}(1:4)); 22 | dot_q(ii) = [rqd(q{i}(1:4),qd{i}(1:3) )]; 23 | 24 | otherwise 25 | dot_q(ii) = qd{i}; 26 | end 27 | end 28 | end 29 | 30 | 31 | -------------------------------------------------------------------------------- /v3/utility/configuration/normalizeConfVec.m: -------------------------------------------------------------------------------- 1 | function q = normalizeConfVec(model,q) 2 | 3 | if ~isfield(model,'nq') 4 | model = postProcessModel(model); 5 | end 6 | if ~iscell(q) 7 | [q] = confVecToCell(model,q); 8 | end 9 | 10 | for i = 1:model.NB 11 | switch model.jtype{i} 12 | case {'Fb','S'} 13 | q{i}(1:4) = q{i}(1:4)/norm( q{i}(1:4) ); 14 | otherwise 15 | q{i} = q{i}; 16 | end 17 | end 18 | q = cell2mat(q); 19 | end 20 | 21 | 22 | -------------------------------------------------------------------------------- /v3/utility/finite_diff/complexStepJacobian.m: -------------------------------------------------------------------------------- 1 | function [J, evals, steps] = complexStepJacobian(f, x, step) 2 | if nargin == 2 3 | step = eps; 4 | end 5 | f0 = f(x); 6 | f0_vec = f0(:); 7 | 8 | n = length(f0_vec); 9 | m = length(x); 10 | 11 | J = zeros(n,m); 12 | for ind = 1:m 13 | e = zeros(m,1); 14 | e(ind) = 1; 15 | steps(:,ind) = x+1i*e*step; 16 | evals(:,ind) = reshape( f(x+1i*e*step), [], 1); 17 | 18 | J(:,ind) = imag( evals(:,ind) )/step; 19 | end 20 | 21 | evals = reshape(evals, [size(f0) length(x)]); 22 | J = reshape(J, [size(f0) length(x)]); 23 | if size(J,2) == 1 24 | J = squeeze(J); 25 | end 26 | end -------------------------------------------------------------------------------- /v3/utility/finite_diff/finiteDiff.m: -------------------------------------------------------------------------------- 1 | function df = finiteDiff(f, x, dx, step) 2 | if nargin == 3 3 | step = sqrt(eps); 4 | end 5 | df = (f(x+step*dx)-f(x))/step; 6 | end -------------------------------------------------------------------------------- /v3/utility/finite_diff/finiteDiffJacobian.m: -------------------------------------------------------------------------------- 1 | function J = finiteDiffJacobian(f, x, step) 2 | if nargin == 2 3 | step = sqrt(eps); 4 | end 5 | f0 = f(x); 6 | n = length(f(x)); 7 | m = length(x); 8 | 9 | J = zeros(n,m); 10 | for ind = 1:m 11 | e = zeros(m,1); 12 | e(ind) = 1; 13 | J(:,ind) = real(f(x+e*step) - f0)/step; 14 | end 15 | end -------------------------------------------------------------------------------- /v3/utility/lie_group_tools/rightJacobianOfExp.m: -------------------------------------------------------------------------------- 1 | function B = rightJacobianOfExp(ad_phi) 2 | B = eye(size(ad_phi,1)); 3 | for i = 1:40 4 | B = B + ad_phi^i *(-1)^i / factorial(i+1); 5 | end 6 | end 7 | 8 | -------------------------------------------------------------------------------- /v3/utility/logdetBregmanDivergence.m: -------------------------------------------------------------------------------- 1 | function d = logdetBregmanDivergence(P,Q) 2 | d = -log( det(P) / det(Q)) + trace(Q\P) - length(P); 3 | end -------------------------------------------------------------------------------- /v3/utility/outputSelect.m: -------------------------------------------------------------------------------- 1 | function out = outputSelect(varargin) 2 | num = varargin{1}; 3 | f = varargin{2}; 4 | eval_str = 'f('; 5 | for i = 3:(nargin) 6 | eval_str = [eval_str 'varargin{' num2str(i) '},']; 7 | end 8 | eval_str = [eval_str(1:end-1) ')']; 9 | 10 | assert(num <= 8,'requires num <= 8'); 11 | 12 | if num ==1 13 | out = eval(eval_str); 14 | elseif num==2 15 | [~, out] = eval(eval_str); 16 | elseif num==3 17 | [~,~,out] = eval(eval_str); 18 | elseif num==4 19 | [~,~,~,out] = eval(eval_str); 20 | elseif num==5 21 | [~,~,~,~,out] = eval(eval_str); 22 | elseif num==6 23 | [~,~,~,~,~,out] = eval(eval_str); 24 | elseif num==7 25 | [~,~,~,~,~,~,out] = eval(eval_str); 26 | elseif num==8 27 | [~,~,~,~,~,~,~,out] = eval(eval_str); 28 | end 29 | end -------------------------------------------------------------------------------- /v3/utility/postProcessModel.m: -------------------------------------------------------------------------------- 1 | function model = postProcessModel(model) 2 | qi = 0; 3 | vi = 0; 4 | ai = 0; 5 | 6 | if ~isfield(model,'has_rotor') 7 | model.has_rotor = zeros(model.NB,1); 8 | end 9 | 10 | 11 | model.nq = []; 12 | model.nv = []; 13 | model.qinds = {}; 14 | model.vinds = {}; 15 | model.ancestors = {}; 16 | model.ancestor_vinds = {}; 17 | model.subtree_vinds = {}; 18 | 19 | 20 | for i = 1:model.NB 21 | [nqi, nvi] = jinfo( model.jtype{i} ); 22 | model.qinds{i} = qi+1 : qi+nqi; 23 | model.vinds{i} = vi+1 : vi+nvi; 24 | 25 | if model.parent(i) == 0 26 | model.ancestors{i} = []; 27 | model.ancestor_vinds{i} = []; 28 | else 29 | model.ancestors{i} = model.ancestors{ model.parent(i)}; 30 | model.ancestor_vinds{i} = model.ancestor_vinds{ model.parent(i)}; 31 | end 32 | model.ancestors{i} = [model.ancestors{i} i]; 33 | model.ancestor_vinds{i} = [model.ancestor_vinds{i} model.vinds{i}]; 34 | 35 | if model.has_rotor(i) 36 | model.rotor_param_inds{i} = ai+1 : ai+10; 37 | ai = ai+10; 38 | end 39 | 40 | model.nq(i) = nqi; 41 | model.nv(i) = nvi; 42 | 43 | qi = qi + nqi; 44 | vi = vi + nvi; 45 | model.subtree_vinds{i} = []; 46 | model.successor_vinds{i} = []; 47 | end 48 | 49 | for i = model.NB:-1:1 50 | ii = model.vinds{i}; 51 | model.subtree_vinds{i} = [ii model.subtree_vinds{i}]; 52 | if model.parent(i) > 0 53 | p = model.parent(i); 54 | model.subtree_vinds{p} = [model.subtree_vinds{i} model.subtree_vinds{p}]; 55 | model.successor_vinds{p} = [ii model.successor_vinds{i} model.successor_vinds{p}]; 56 | end 57 | end 58 | 59 | model.NV = vi; 60 | model.NQ = qi; 61 | model.NB_rot = ai/10; 62 | end 63 | 64 | -------------------------------------------------------------------------------- /v3/utility/random/randomAdSE3.m: -------------------------------------------------------------------------------- 1 | function X = randomAdSE3() 2 | p = randn(3,1); 3 | R = randomRotation(); 4 | X = plux(R,p); 5 | end 6 | -------------------------------------------------------------------------------- /v3/utility/random/randomInertia.m: -------------------------------------------------------------------------------- 1 | function a = randomInertia() 2 | 3 | a = inertiaVecToMat(randomInertialParams()); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /v3/utility/random/randomInertialParams.m: -------------------------------------------------------------------------------- 1 | function a = randomInertialParams() 2 | J = rand(4,4)-.5; 3 | J = (J+J')/2; 4 | 5 | J = expm(J); 6 | a = pinertiaToVec(J); 7 | end 8 | 9 | -------------------------------------------------------------------------------- /v3/utility/random/randomPinertia.m: -------------------------------------------------------------------------------- 1 | function J = randomPinertia() 2 | J = rand(4,4)-.5; 3 | J = (J+J'); 4 | J = expm(J); 5 | end 6 | 7 | -------------------------------------------------------------------------------- /v3/utility/random/randomPositiveDefinite.m: -------------------------------------------------------------------------------- 1 | function A = randomPositiveDefinite(n) 2 | A = expm( randomSymmetric(n) ); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /v3/utility/random/randomQuat.m: -------------------------------------------------------------------------------- 1 | function q = randomQuat() 2 | q = randn(4,1); 3 | q = q/norm(q); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /v3/utility/random/randomRotation.m: -------------------------------------------------------------------------------- 1 | function R = randomRotation() 2 | r = randn(3,1); 3 | r = r/norm(r); 4 | th = rand()*pi; 5 | R = expm(skew(r*th)); 6 | end 7 | -------------------------------------------------------------------------------- /v3/utility/random/randomSE3.m: -------------------------------------------------------------------------------- 1 | function T = randomSE3() 2 | p = randn(3,1); 3 | R = randomRotation(); 4 | T = [R p ; 0 0 0 1]; 5 | end 6 | -------------------------------------------------------------------------------- /v3/utility/random/randomSkew.m: -------------------------------------------------------------------------------- 1 | function A = randomSkew(n) 2 | if nargin == 0 3 | n = 3; 4 | end 5 | 6 | A = rand(n)-.5; 7 | A = A-A'; 8 | end 9 | 10 | -------------------------------------------------------------------------------- /v3/utility/random/randomSymmetric.m: -------------------------------------------------------------------------------- 1 | function A = randomSymmetric(n) 2 | A = rand(n)-.5; 3 | A = A+A'; 4 | end 5 | 6 | -------------------------------------------------------------------------------- /v3/utility/random/random_se3.m: -------------------------------------------------------------------------------- 1 | function T = random_se3() 2 | p = randn(3,1); 3 | W = randomSkew(3); 4 | T = [W p ; 0 0 0 0]; 5 | end -------------------------------------------------------------------------------- /v3/utility/symbolic/symbolicCartesian.m: -------------------------------------------------------------------------------- 1 | function [ param_vec, param_struct ] = symbolicCartesian( symbol, body_number ) 2 | % symbolicCartesian returns a symbolic cartesian vector 3 | % 4 | % Input: (optional) body_number (let's call it i) 5 | % 6 | % 7 | % Ouput: param_vec = [symbol_x_i symbol_y_i symbol_z_i] (symbolic) 8 | % 9 | % params_struct (same, but as structure with elements symbolx, symboly,symbolz) 10 | 11 | if nargin <= 1 12 | append = ''; 13 | else 14 | append = ['_' num2str(body_number)]; 15 | end 16 | 17 | sx = sym([symbol '_x' append], 'real'); 18 | sy = sym([symbol '_y' append], 'real'); 19 | sz = sym([symbol '_z' append], 'real'); 20 | 21 | param_vec = [sx sy sz]'; 22 | 23 | param_struct.([symbol 'x']) = sx; 24 | param_struct.([symbol 'y']) = sy; 25 | param_struct.([symbol 'z']) = sz; 26 | 27 | end 28 | 29 | -------------------------------------------------------------------------------- /v3/utility/symbolic/symbolicForce.m: -------------------------------------------------------------------------------- 1 | function [ param_vec, param_struct ] = symbolicForce( body_number ) 2 | % symbolicVelocity returns a symbolic spatial force vector 3 | % 4 | % Input: (optional) body_number (let's call it i) 5 | % 6 | % 7 | % Ouput: param_vec = [n_x_i n_y_i n_z_i f_x_i f_y_i f_z_i] (symbolic) 8 | % 9 | % params_struct (same, but as structure with elements nx, ny,nz, fx,fy,fz etc.) 10 | 11 | if nargin == 0 12 | [n, n_struct] = symbolicCartesian('n'); 13 | [f, f_struct] = symbolicCartesian('f'); 14 | else 15 | [n, n_struct] = symbolicCartesian('n',body_number); 16 | [f, f_struct] = symbolicCartesian('f',body_number); 17 | end 18 | 19 | param_vec = [n ; f]; 20 | param_struct = n_struct; 21 | entries = fields(f_struct); 22 | 23 | for i = 1:length(entries) 24 | field_name = entries{i}; 25 | param_struct.(field_name) = f_struct.(field_name); 26 | end 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /v3/utility/symbolic/symbolicInertia.m: -------------------------------------------------------------------------------- 1 | function I = symbolicInertia( body_number ) 2 | % symbolicInertia returns a symbolic Inertia matrix 3 | % 4 | % Input: (optional) body_number (let's call it i) 5 | % 6 | % 7 | % Ouput: symbolic 6x6 spatial inertia 8 | 9 | if nargin == 0 10 | I = inertiaVecToMat( symbolicInertialParams() ); 11 | else 12 | I = inertiaVecToMat( symbolicInertialParams(body_number) ); 13 | end 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /v3/utility/symbolic/symbolicInertialParams.m: -------------------------------------------------------------------------------- 1 | function [ param_vec, param_struct ] = symbolicInertialParams( body_number ) 2 | % symbolicInertialParams returns a symbolic vector of 10 inertial parameters 3 | % 4 | % Input: (optional) body_number (let's call it i) 5 | % 6 | % 7 | % Ouput: param_vec = [m_i h_x_i h_y_i h_z_i I_xx_i I_yy_i I_zz_i I_xy_i I_xz_i 8 | % I_yz_i]' (symbolic) 9 | % 10 | % params_struct (same, but as structure with elements m, hx, etc.) 11 | 12 | if nargin == 0 13 | append = ''; 14 | else 15 | append = ['_' num2str(body_number)]; 16 | end 17 | 18 | m = sym(['m' append], 'real'); 19 | hx = sym(['h_x' append], 'real'); 20 | hy = sym(['h_y' append], 'real'); 21 | hz = sym(['h_z' append], 'real'); 22 | Ixx = sym(['I_xx' append], 'real'); 23 | Iyy = sym(['I_yy' append], 'real'); 24 | Izz = sym(['I_zz' append], 'real'); 25 | Iyz = sym(['I_yz' append], 'real'); 26 | Ixz = sym(['I_xz' append], 'real'); 27 | Ixy = sym(['I_xy' append], 'real'); 28 | 29 | param_vec = [m hx hy hz Ixx Iyy Izz Iyz Ixz Ixy]'; 30 | 31 | param_struct.m = m; 32 | param_struct.hx = hx; 33 | param_struct.hy = hy; 34 | param_struct.hz = hz; 35 | param_struct.Ixx = Ixx; 36 | param_struct.Iyy = Iyy; 37 | param_struct.Izz = Izz; 38 | param_struct.Iyz = Iyz; 39 | param_struct.Ixz = Ixz; 40 | param_struct.Ixy = Ixy; 41 | 42 | end 43 | 44 | -------------------------------------------------------------------------------- /v3/utility/symbolic/symbolicVelocity.m: -------------------------------------------------------------------------------- 1 | function [ param_vec, param_struct ] = symbolicVelocity( body_number ) 2 | % symbolicVelocity returns a symbolic spatial velocity vector 3 | % 4 | % Input: (optional) body_number (let's call it i) 5 | % 6 | % 7 | % Ouput: param_vec = [omega_x_i omega_y_i omega_z_i v_x_i v_y_i v_z_i] (symbolic) 8 | % 9 | % params_struct (same, but as structure with elements wx, wy,wz, vx,vy,vz etc.) 10 | 11 | if nargin == 0 12 | append = ''; 13 | else 14 | append = ['_' num2str(body_number)]; 15 | end 16 | 17 | wx = sym(['omega_x' append], 'real'); 18 | wy = sym(['omega_y' append], 'real'); 19 | wz = sym(['omega_z' append], 'real'); 20 | vx = sym(['v_x' append], 'real'); 21 | vy = sym(['v_y' append], 'real'); 22 | vz = sym(['v_z' append], 'real'); 23 | 24 | 25 | param_vec = [wx wy wz vx vy vz]'; 26 | 27 | param_struct.wx = wx; 28 | param_struct.wy = wy; 29 | param_struct.wz = wz; 30 | param_struct.vx = vx; 31 | param_struct.vy = vy; 32 | param_struct.vz = vz; 33 | 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /v3/utility/tensor/tensorRotR.m: -------------------------------------------------------------------------------- 1 | function tens_out = tensorRotR(tens_in) 2 | 3 | tens_out = permute(tens_in, [1 3 2]); -------------------------------------------------------------------------------- /v3/utility/tensor/tensorRotT.m: -------------------------------------------------------------------------------- 1 | function tens_out = tensorRotT(tens_in) 2 | % takes one by one transpose of tensor - along 1-2 dims 3 | 4 | tens_out = permute(tens_in,[2 1 3]); --------------------------------------------------------------------------------