(* Inverse dynamics for a torso plus a leg using Newton Euler approach *) (* Moment of inertia about center of mass of each link *) (* zero position has torso vertical along Y axis, with torso center of mass at world origin *) (*Deal with time dependence *) SetAttributes [Mtorso, Constant] SetAttributes [Mthigh, Constant] SetAttributes [Mcalf, Constant] SetAttributes [TorsotoHip, Constant] (* should be negative *) SetAttributes [HiptoThighCOM, Constant] (* should be negative *) SetAttributes [HiptoKnee, Constant] (* should be negative *) SetAttributes [KneetoCalfCOM, Constant] (* should be negative *) SetAttributes [KneetoAnkle, Constant] (* should be negative *) SetAttributes [Itorso, Constant] SetAttributes [Ithigh, Constant] SetAttributes [Icalf, Constant] (* location of torso cm is x, y with orientation a0 *) x/: Dt[x, t] = xd xd/: Dt[xd, t] = xdd y/: Dt[y, t] = yd yd/: Dt[yd, t] = ydd a0/: Dt[a0, t] = a0d a0d/: Dt[a0d, t] = a0dd (* joints *) hipL/: Dt[hipL, t] = hipLd hipLd/: Dt[hipLd, t] = hipLdd kneeL/: Dt[kneeL, t] = kneeLd kneeLd/: Dt[kneeLd, t] = kneeLdd (* Y vertical X to right coordinate system. *) gravity = {0, -G} (* Define rotation matrices that allow us to do the forward kinematics. *) (* R01 rotates from the link 1 (torso) frame to the link 0 (world) frame. *) r01={{Cos[a0],-Sin[a0]},{Sin[a0],Cos[a0]}} r10=Transpose[r01] rTorsoHipL={{Cos[hipL],-Sin[hipL]},{Sin[hipL],Cos[hipL]}} rHipLTorso=Transpose[rTorsoHipL] rHipLKneeL={{Cos[kneeL],-Sin[kneeL]},{Sin[kneeL],Cos[kneeL]}} rKneeLHipL=Transpose[rHipLKneeL] (* Define the location of the proximal end of each link (in world coordinates) *) Ptorso = {x, y} PthighL = Ptorso + r01 . {0, TorsotoHip} PcalfL = PthighL + r01 . rTorsoHipL . {0, HiptoKnee} (* Define the location of the center of mass with respect to the proximal end of each link. (in world coordinates) *) CMtorso = {0, 0} CMthighL = r01 . rTorsoHipL . {0, HiptoThighCOM} CMcalfL = r01 . rTorsoHipL . rHipLKneeL . {0, KneetoCalfCOM} (* Location where ground forces applied *) GroundL = r01 . rTorsoHipL . rHipLKneeL . {0, KneetoAnkle} (* Compute the location of the center of mass of each link. *) Qtorso = Ptorso + CMtorso QthighL = PthighL + CMthighL QcalfL = PcalfL + CMcalfL (* Compute the linear velocity and acceleration of each link cm. *) Qtorsod = Dt[Qtorso,t] QthighLd = Dt[QthighL,t] QcalfLd = Dt[QcalfL,t] Qtorsodd = Dt[Qtorsod,t] QthighLdd = Dt[QthighLd,t] QcalfLdd = Dt[QcalfLd,t] (* Compute net link forces. *) Ftorso = Mtorso * Qtorsodd - Mtorso * gravity FthighL = Mthigh * QthighLdd - Mthigh * gravity FcalfL = Mcalf * QcalfLdd - Mcalf * gravity (* Compute the angular velocity of each link. *) Wtorso = Dt[a0,t] WthighL = Wtorso + Dt[hipL,t] WcalfL = WthighL + Dt[kneeL,t] (* Compute the angular acceleration of each link. *) Wtorsod = Dt[Wtorso,t] WthighLd = Dt[WthighL,t] WcalfLd = Dt[WcalfL,t] (*Compute net link torques.*) Ntorso = Itorso*Wtorsod NthighL = Ithigh*WthighLd NcalfL = Icalf*WcalfLd (*Compute forces at the joints.*) FGroundL = {FGroundXL, FGroundYL} FthighLCalfL = FcalfL + FGroundL FtorsoThighL = FthighL + FthighLCalfL FworldTorso = Ftorso + FtorsoThighL (*Define the 2D vector cross product.*) CP[X_,Y_] := X[[1]]*Y[[2]] - X[[2]]*Y[[1]] (*Compute torques at the joints.*) tauKneeL = NcalfL + CP[ CMcalfL, FcalfL ] + CP[ GroundL, FGroundL ] tauHipL = NthighL + tauKneeL + CP[ CMthighL, FthighL ] + CP[ PcalfL-PthighL, FthighLCalfL ] tau0 = Ntorso + tauHipL + CP[ CMtorso, Ftorso ] + CP[ PthighL-Ptorso, FtorsoThighL ] fx = FworldTorso[[1]] fy = FworldTorso[[2]] (**************************************************************) fx = Simplify[Expand[Simplify[Expand[fx, Trig->True]]]] fy = Simplify[Expand[Simplify[Expand[fy, Trig->True]]]] tau0 = Simplify[Expand[Simplify[Expand[tau0, Trig->True]]]] tauHipL = Simplify[Expand[Simplify[Expand[tauHipL, Trig->True]]]] tauKneeL = Simplify[Expand[Simplify[Expand[tauKneeL, Trig->True]]]] (**************************************************************) (**************************************************************) (* Compute terms of H in H*acceleration = stuff *) a00 = Simplify[Expand[Simplify[Coefficient[fx,xdd]]]] a01 = Simplify[Expand[Simplify[Coefficient[fx,ydd]]]] a02 = Simplify[Expand[Simplify[Coefficient[fx,a0dd]]]] a03 = Simplify[Expand[Simplify[Coefficient[fx,hipLdd]]]] a04 = Simplify[Expand[Simplify[Coefficient[fx,kneeLdd]]]] CForm[a00] Mcalf + Mthigh + Mtorso CForm[a01] 0 CForm[a02] -((Mcalf + Mthigh)*TorsotoHip*Cos(a0)) - (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*Cos(a0 + hipL) - KneetoCalfCOM*Mcalf*Cos(a0 + hipL + kneeL) CForm[a03] -((HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*Cos(a0 + hipL)) - KneetoCalfCOM*Mcalf*Cos(a0 + hipL + kneeL) CForm[a04] -(KneetoCalfCOM*Mcalf*Cos(a0 + hipL + kneeL)) CForm[Simplify[Expand[Simplify[Expand[fx - xdd*a00 - ydd*a01 - a0dd*a02 - hipLdd*a03 - kneeLdd*a04]]]]] FGroundXL + Power(a0d,2)*(Mcalf + Mthigh)*TorsotoHip*Sin(a0) + Power(a0d + hipLd,2)*(HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)* Sin(a0 + hipL) + Power(a0d,2)*KneetoCalfCOM*Mcalf* Sin(a0 + hipL + kneeL) + 2*a0d*hipLd*KneetoCalfCOM*Mcalf* Sin(a0 + hipL + kneeL) + Power(hipLd,2)*KneetoCalfCOM*Mcalf* Sin(a0 + hipL + kneeL) + 2*a0d*kneeLd*KneetoCalfCOM*Mcalf* Sin(a0 + hipL + kneeL) + 2*hipLd*kneeLd*KneetoCalfCOM*Mcalf* Sin(a0 + hipL + kneeL) + Power(kneeLd,2)*KneetoCalfCOM*Mcalf* Sin(a0 + hipL + kneeL) (**************************************************************) a10 = Simplify[Expand[Simplify[Coefficient[fy,xdd]]]] a11 = Simplify[Expand[Simplify[Coefficient[fy,ydd]]]] a12 = Simplify[Expand[Simplify[Coefficient[fy,a0dd]]]] a13 = Simplify[Expand[Simplify[Coefficient[fy,hipLdd]]]] a14 = Simplify[Expand[Simplify[Coefficient[fy,kneeLdd]]]] CForm[a10] 0 CForm[a11] Mcalf + Mthigh + Mtorso CForm[a12] -((Mcalf + Mthigh)*TorsotoHip*Sin(a0)) - (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*Sin(a0 + hipL) - KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL) CForm[a13] -((HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*Sin(a0 + hipL)) - KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL) CForm[a14] -(KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL)) CForm[Simplify[Expand[Simplify[Expand[fy - xdd*a10 - ydd*a11 - a0dd*a12 - hipLdd*a13 - kneeLdd*a14]]]]] FGroundYL + G*Mcalf + G*Mthigh + G*Mtorso - Power(a0d,2)*(Mcalf + Mthigh)*TorsotoHip*Cos(a0) - Power(a0d + hipLd,2)*(HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)* Cos(a0 + hipL) - Power(a0d,2)*KneetoCalfCOM*Mcalf* Cos(a0 + hipL + kneeL) - 2*a0d*hipLd*KneetoCalfCOM*Mcalf* Cos(a0 + hipL + kneeL) - Power(hipLd,2)*KneetoCalfCOM*Mcalf* Cos(a0 + hipL + kneeL) - 2*a0d*kneeLd*KneetoCalfCOM*Mcalf* Cos(a0 + hipL + kneeL) - 2*hipLd*kneeLd*KneetoCalfCOM*Mcalf* Cos(a0 + hipL + kneeL) - Power(kneeLd,2)*KneetoCalfCOM*Mcalf* Cos(a0 + hipL + kneeL) (**************************************************************) a20 = Simplify[Expand[Simplify[Coefficient[tau0,xdd]]]] a21 = Simplify[Expand[Simplify[Coefficient[tau0,ydd]]]] a22 = Simplify[Expand[Simplify[Coefficient[tau0,a0dd]]]] a23 = Simplify[Expand[Simplify[Coefficient[tau0,hipLdd]]]] a24 = Simplify[Expand[Simplify[Coefficient[tau0,kneeLdd]]]] CForm[a20] -((Mcalf + Mthigh)*TorsotoHip*Cos(a0)) - (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*Cos(a0 + hipL) - KneetoCalfCOM*Mcalf*Cos(a0 + hipL + kneeL) CForm[a21] -((Mcalf + Mthigh)*TorsotoHip*Sin(a0)) - (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*Sin(a0 + hipL) - KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL) CForm[a22] Icalf + Ithigh + Itorso + Power(HiptoKnee,2)*Mcalf + Power(KneetoCalfCOM,2)*Mcalf + Power(HiptoThighCOM,2)*Mthigh + Mcalf*Power(TorsotoHip,2) + Mthigh*Power(TorsotoHip,2) + 2*(HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*TorsotoHip*Cos(hipL) + 2*HiptoKnee*KneetoCalfCOM*Mcalf*Cos(kneeL) + 2*KneetoCalfCOM*Mcalf*TorsotoHip*Cos(hipL + kneeL) CForm[a23] Icalf + Ithigh + Power(HiptoKnee,2)*Mcalf + Power(KneetoCalfCOM,2)*Mcalf + Power(HiptoThighCOM,2)*Mthigh + (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*TorsotoHip*Cos(hipL) + 2*HiptoKnee*KneetoCalfCOM*Mcalf*Cos(kneeL) + KneetoCalfCOM*Mcalf*TorsotoHip*Cos(hipL + kneeL) CForm[a24] Icalf + Power(KneetoCalfCOM,2)*Mcalf + HiptoKnee*KneetoCalfCOM*Mcalf*Cos(kneeL) + KneetoCalfCOM*Mcalf*TorsotoHip*Cos(hipL + kneeL) CForm[Simplify[Expand[Simplify[Expand[tau0 - xdd*a20 - ydd*a21 - a0dd*a22 - hipLdd*a23 - kneeLdd*a24]]]]] -(FGroundXL*TorsotoHip*Cos(a0)) - FGroundXL*HiptoKnee*Cos(a0 + hipL) - FGroundXL*KneetoAnkle*Cos(a0 + hipL + kneeL) - FGroundYL*TorsotoHip*Sin(a0) - G*Mcalf*TorsotoHip*Sin(a0) - G*Mthigh*TorsotoHip*Sin(a0) - 2*a0d*hipLd*HiptoKnee*Mcalf*TorsotoHip*Sin(hipL) - Power(hipLd,2)*HiptoKnee*Mcalf*TorsotoHip*Sin(hipL) - 2*a0d*hipLd*HiptoThighCOM*Mthigh*TorsotoHip*Sin(hipL) - Power(hipLd,2)*HiptoThighCOM*Mthigh*TorsotoHip*Sin(hipL) - FGroundYL*HiptoKnee*Sin(a0 + hipL) - G*HiptoKnee*Mcalf*Sin(a0 + hipL) - G*HiptoThighCOM*Mthigh*Sin(a0 + hipL) - 2*a0d*HiptoKnee*kneeLd*KneetoCalfCOM*Mcalf*Sin(kneeL) - 2*hipLd*HiptoKnee*kneeLd*KneetoCalfCOM*Mcalf*Sin(kneeL) - HiptoKnee*Power(kneeLd,2)*KneetoCalfCOM*Mcalf*Sin(kneeL) - 2*a0d*hipLd*KneetoCalfCOM*Mcalf*TorsotoHip*Sin(hipL + kneeL) - Power(hipLd,2)*KneetoCalfCOM*Mcalf*TorsotoHip*Sin(hipL + kneeL) - 2*a0d*kneeLd*KneetoCalfCOM*Mcalf*TorsotoHip*Sin(hipL + kneeL) - 2*hipLd*kneeLd*KneetoCalfCOM*Mcalf*TorsotoHip*Sin(hipL + kneeL) - Power(kneeLd,2)*KneetoCalfCOM*Mcalf*TorsotoHip*Sin(hipL + kneeL) - FGroundYL*KneetoAnkle*Sin(a0 + hipL + kneeL) - G*KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL) (**************************************************************) a30 = Simplify[Expand[Simplify[Coefficient[tauHipL,xdd]]]] a31 = Simplify[Expand[Simplify[Coefficient[tauHipL,ydd]]]] a32 = Simplify[Expand[Simplify[Coefficient[tauHipL,a0dd]]]] a33 = Simplify[Expand[Simplify[Coefficient[tauHipL,hipLdd]]]] a34 = Simplify[Expand[Simplify[Coefficient[tauHipL,kneeLdd]]]] CForm[a30] -((HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*Cos(a0 + hipL)) - KneetoCalfCOM*Mcalf*Cos(a0 + hipL + kneeL) CForm[a31] -((HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*Sin(a0 + hipL)) - KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL) CForm[a32] Icalf + Ithigh + Power(HiptoKnee,2)*Mcalf + Power(KneetoCalfCOM,2)*Mcalf + Power(HiptoThighCOM,2)*Mthigh + (HiptoKnee*Mcalf + HiptoThighCOM*Mthigh)*TorsotoHip*Cos(hipL) + 2*HiptoKnee*KneetoCalfCOM*Mcalf*Cos(kneeL) + KneetoCalfCOM*Mcalf*TorsotoHip*Cos(hipL + kneeL) CForm[a33] Icalf + Ithigh + Power(HiptoKnee,2)*Mcalf + Power(KneetoCalfCOM,2)*Mcalf + Power(HiptoThighCOM,2)*Mthigh + 2*HiptoKnee*KneetoCalfCOM*Mcalf*Cos(kneeL) CForm[a34] Icalf + Power(KneetoCalfCOM,2)*Mcalf + HiptoKnee*KneetoCalfCOM*Mcalf*Cos(kneeL) CForm[Simplify[Expand[Simplify[Expand[tauHipL - xdd*a30 - ydd*a31 - a0dd*a32 - hipLdd*a33 - kneeLdd*a34]]]]] -(FGroundXL*HiptoKnee*Cos(a0 + hipL)) - FGroundXL*KneetoAnkle*Cos(a0 + hipL + kneeL) + Power(a0d,2)*HiptoKnee*Mcalf*TorsotoHip*Sin(hipL) + Power(a0d,2)*HiptoThighCOM*Mthigh*TorsotoHip*Sin(hipL) - FGroundYL*HiptoKnee*Sin(a0 + hipL) - G*HiptoKnee*Mcalf*Sin(a0 + hipL) - G*HiptoThighCOM*Mthigh*Sin(a0 + hipL) - 2*a0d*HiptoKnee*kneeLd*KneetoCalfCOM*Mcalf*Sin(kneeL) - 2*hipLd*HiptoKnee*kneeLd*KneetoCalfCOM*Mcalf*Sin(kneeL) - HiptoKnee*Power(kneeLd,2)*KneetoCalfCOM*Mcalf*Sin(kneeL) + Power(a0d,2)*KneetoCalfCOM*Mcalf*TorsotoHip*Sin(hipL + kneeL) - FGroundYL*KneetoAnkle*Sin(a0 + hipL + kneeL) - G*KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL) (**************************************************************) a40 = Simplify[Expand[Simplify[Coefficient[tauKneeL,xdd]]]] a41 = Simplify[Expand[Simplify[Coefficient[tauKneeL,ydd]]]] a42 = Simplify[Expand[Simplify[Coefficient[tauKneeL,a0dd]]]] a43 = Simplify[Expand[Simplify[Coefficient[tauKneeL,hipLdd]]]] a44 = Simplify[Expand[Simplify[Coefficient[tauKneeL,kneeLdd]]]] CForm[a40] -(KneetoCalfCOM*Mcalf*Cos(a0 + hipL + kneeL)) CForm[a41] -(KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL)) CForm[a42] Icalf + Power(KneetoCalfCOM,2)*Mcalf + HiptoKnee*KneetoCalfCOM*Mcalf*Cos(kneeL) + KneetoCalfCOM*Mcalf*TorsotoHip*Cos(hipL + kneeL) CForm[a43] Icalf + Power(KneetoCalfCOM,2)*Mcalf + HiptoKnee*KneetoCalfCOM*Mcalf*Cos(kneeL) CForm[a44] Icalf + Power(KneetoCalfCOM,2)*Mcalf CForm[Simplify[Expand[Simplify[Expand[tauKneeL - xdd*a40 - ydd*a41 - a0dd*a42 - hipLdd*a43 - kneeLdd*a44]]]]] -(FGroundXL*KneetoAnkle*Cos(a0 + hipL + kneeL)) + Power(a0d + hipLd,2)*HiptoKnee*KneetoCalfCOM*Mcalf*Sin(kneeL) + Power(a0d,2)*KneetoCalfCOM*Mcalf*TorsotoHip*Sin(hipL + kneeL) - FGroundYL*KneetoAnkle*Sin(a0 + hipL + kneeL) - G*KneetoCalfCOM*Mcalf*Sin(a0 + hipL + kneeL) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************) (**************************************************************)