Follow PR #11

ami-iit · Jun 17, 2022 · 5462386 · 5462386
1 parent f4e84d0
commit 5462386
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 5 deletions.
diff --git a/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m b/+mystica/+state/@StateDynMBody/get_mBodyVelAcc0_from_motorsCurrent.m
@@ -36,7 +36,7 @@
             kiFeedbackJcV = input.kFeedbackJcV(2);
 
             mBodyTwist_0     = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel);
-            mBodyVelQuat_0   = sparse(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
+            mBodyVelQuat_0   = sparse(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
 
             Jc        = obj.Jc( obj.linIndRowJc,:);
             dJc       = sparse(obj.csdFn.dJc(obj.mBodyPosVel_0));
@@ -75,7 +75,7 @@
                 X = sol.value(obj.optiVar.X);
 
                 mBodyTwist_0     = obj.mBodyPosVel_0(input.model.selector.indexes_mBodyTwist_from_mBodyPosVel);
-                mBodyVelQuat_0   = sparse(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
+                mBodyVelQuat_0   = sparse(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
 
                 mBodyTwAcc_0          = X(1:input.model.constants.mBodyTwist,1);
                 jointsWrenchConstr_PJ = X(input.model.constants.mBodyTwist+1:end,1);

diff --git a/+mystica/+state/@StateKinMBody/getKinematicQuantities.m b/+mystica/+state/@StateKinMBody/getKinematicQuantities.m
@@ -40,7 +40,7 @@ function getKinematicQuantities(obj,model)
     linkVelQuat_0 = casadi.Function('f',{linkPosQuat_0,linkTwist_0},{[mystica.rbm.getPosGivenTwist(linkTwist_0);...
              mystica.rbm.getDotQuatGivenAngVel0(mystica.rbm.getQuatGivenPosQuat(linkPosQuat_0),mystica.rbm.getAngVelGivenTwist(linkTwist_0),kBaum)]});
     mBodyVelQuat0_csdFn_matrix = linkVelQuat_0.map(model.nLink);
-    obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0 = casadi.Function('get_mBodyVelQuat_from_mBodyTwist',{obj.csdSy.mBodyPosQuat_0,mBodyTwist_0,kBaum},{reshape(mBodyVelQuat0_csdFn_matrix(...
+    obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0 = casadi.Function('get_mBodyVelQuat0_from_mBodyTwist0',{obj.csdSy.mBodyPosQuat_0,mBodyTwist_0,kBaum},{reshape(mBodyVelQuat0_csdFn_matrix(...
         reshape(obj.csdSy.mBodyPosQuat_0,model.constants.linkPosQuat,model.nLink),...
         reshape(mBodyTwist_0,model.constants.linkTwist,model.nLink)),model.constants.mBodyPosQuat,1)});
 

diff --git a/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_mBodyTwist0.m b/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_mBodyTwist0.m
@@ -17,7 +17,7 @@
     end
 
     if isnumeric(obj.mBodyPosQuat_0) && isnumeric(input.mBodyTwist_0) && isnumeric(input.kBaum)
-        mBodyVelQuat_0 = full(mystica_stateKin('get_mBodyVelQuat_from_mBodyTwist',obj.mBodyPosQuat_0,input.mBodyTwist_0,input.kBaum));
+        mBodyVelQuat_0 = full(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',obj.mBodyPosQuat_0,input.mBodyTwist_0,input.kBaum));
     else
         mBodyVelQuat_0 = full(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,input.mBodyTwist_0,input.kBaum));
     end

diff --git a/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_motorsAngVel.m b/+mystica/+state/@StateKinMBody/get_mBodyVelQuat0_from_motorsAngVel.m
@@ -17,7 +17,7 @@
     mBodyTwist_0 = obj.nullJc_mBodyTwist_0 * invZact * input.motorsAngVel;
 
     if isnumeric(obj.mBodyPosQuat_0) && isnumeric(mBodyTwist_0) && isnumeric(input.kBaum)
-        mBodyVelQuat_0 = full(mystica_stateKin('get_mBodyVelQuat_from_mBodyTwist',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
+        mBodyVelQuat_0 = full(mystica_stateKin('get_mBodyVelQuat0_from_mBodyTwist0',obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
     else
         mBodyVelQuat_0 = full(obj.csdFn.get_mBodyVelQuat0_from_mBodyTwist0(obj.mBodyPosQuat_0,mBodyTwist_0,input.kBaum));
     end