Dear AnyBody,
I have a problem with unexpected kinematic constraint violations during the inverse dynamic optimization. I have manually added a front foot segment to the original lower extremity foot, using a spherical joint. Similar to the hip joint, I export the mid foot joint angles during motion optimization and drive them during the inverse dynamics optimization. I have added all relevant AnyScript below, and it is running at 50Hz, which I believe should avoid any issues caused by a too big step time.
The problem is the following: The kinematic optimization for the inverse dynamic model only works when the rotation measure type for the MtPhal3 (see below) is set to RotVector. In which case both the kinematic and inverse dynamic analysis works like a charm. However, as soon as I use RotAxesAngles, I always get the following error at some point (and always Constraint no. #8):
Constraint no. #8 in ‘Main.Studies.InverseDynamicStudy.ModelEnvironmentConnection.JointsAndDrivers.JntDriverLeftLegTD’ above error tolerance 0.000001, error = 0.000030.
Constraint no. #8 corresponds to the second Euler angle, around the floating axis according to Grood&Suntay. Except for the spherical joint, there are no other constraints on my front foot segment and the kinematic error always is this small (lowering the kinematic tolerance therefore helps overcoming this issue).
My questions are the following:
1\ How is it possible that the constraint cannot be satisfied (when using RotAxesAngles), if nothing is preventing the front foot from achieving the correct pose?
- To me it seems that this is some kind of numerical issue, where the driver has a hard time solving the correct pose of my front foot segment in order to achieve the correct second Euler angle because it is very sensitive to changes in the other to angles.
2\ Is there a solution where I can keep using RotAxesAngles (as it improves interpretation of both angles and joint muscle forces)?
Kind regards,
Bart
MODEL (I include them at different locations of the model tree, so I hope the internal references make sense):
Definition of the joint:
Jnt = {
AnySphericalJoint MtPhal3 = {
AnyRefFrame &refFoot = refSegFolder.Foot.MtPhal3Joint;
AnyRefFrame &refFrontFoot = refSegFolder.FrontFoot.MtPhal3Joint;
AnyFolder &refSegFolder = …Seg;
// Create Kinematic measures for Interface and joint mucles
AnyKinMeasureOrg RotNodeZ = {
AnyKinRotational MtPhal3RotMeasure ={
AnyRefNode &FootNode = …Seg.Foot.MtPhal3Joint.RotNode;
AnyRefNode &FrontFootNode = …Seg.FrontFoot.MtPhal3Joint.RotNode;
Axis1 = z; Axis2 = x; Axis3 = y;
Type=RotVector;
};
MeasureOrganizer={0};
};
AnyKinMeasureOrg RotNodeX = {
AnyKinRotational &MtPhal3RotMeasure =.RotNodeZ.MtPhal3RotMeasure;
MeasureOrganizer={1};
};
AnyKinMeasureOrg RotNodeY = {
AnyKinRotational &MtPhal3RotMeasure =.RotNodeZ.MtPhal3RotMeasure;
MeasureOrganizer={2};
};
};
}; // end Jnt folder
Interface:
Right = {
AnyKinMeasureOrg &MidFootRotNodeZ = …Right.Leg.Jnt.MtPhal3.RotNodeZ;
AnyKinMeasureOrg &MidFootRotNodeX = …Right.Leg.Jnt.MtPhal3.RotNodeX;
AnyKinMeasureOrg &MidFootRotNodeY = …Right.Leg.Jnt.MtPhal3.RotNodeY;
}; // end Right interface folder
Left = {
AnyKinMeasureOrg &MidFootRotNodeZ = …Left.Leg.Jnt.MtPhal3.RotNodeZ;
AnyKinMeasureOrg &MidFootRotNodeX = …Left.Leg.Jnt.MtPhal3.RotNodeX;
AnyKinMeasureOrg &MidFootRotNodeY = …Left.Leg.Jnt.MtPhal3.RotNodeY;
}; // end Left interface folder
Export of joint angles (druing motion optimization):
#if LEFT_LEG_TD == 1
AnyOutputFile OutputFile2 = {
FileName = RESULTS_PATH +Main.TrialSpecificData.NameOfFile+"-output-euler-leftlegtd.txt";
SepSign = " ";
AnyVector HipFlexion = ..BodyModel.Interface.Left.HipFlexion.Pos;
AnyVector HipAbduction = ..BodyModel.Interface.Left.HipAbduction.Pos;
AnyVector HipExternalRotation = ..BodyModel.Interface.Left.HipExternalRotation.Pos;
AnyVector KneeFlexion = ..BodyModel.Interface.Left.KneeFlexion.Pos;
AnyVector AnklePlantarFlexion = ..BodyModel.Interface.Left.AnklePlantarFlexion.Pos;
AnyVector SubTalarEversion = ..BodyModel.Interface.Left.SubTalarEversion.Pos;
AnyVector LeftMidFootRotNodeZ = ..BodyModel.Interface.Left.MidFootRotNodeZ.Pos;
AnyVector LeftMidFootRotNodeX = ..BodyModel.Interface.Left.MidFootRotNodeX.Pos;
AnyVector LeftMidFootRotNodeY = ..BodyModel.Interface.Left.MidFootRotNodeY.Pos;
};
#endif
#if RIGHT_LEG_TD == 1
AnyOutputFile OutputFile3 = {
FileName = RESULTS_PATH +Main.TrialSpecificData.NameOfFile+"-output-euler-rightlegtd.txt";
SepSign = " ";
AnyVector HipFlexion = ..BodyModel.Interface.Right.HipFlexion.Pos;
AnyVector HipAbduction = ..BodyModel.Interface.Right.HipAbduction.Pos;
AnyVector HipExternalRotation = ..BodyModel.Interface.Right.HipExternalRotation.Pos;
AnyVector KneeFlexion = ..BodyModel.Interface.Right.KneeFlexion.Pos;
AnyVector AnklePlantarFlexion = ..BodyModel.Interface.Right.AnklePlantarFlexion.Pos;
AnyVector SubTalarEversion = ..BodyModel.Interface.Right.SubTalarEversion.Pos;
AnyVector RightMidFootRotNodeZ = ..BodyModel.Interface.Right.MidFootRotNodeZ.Pos;
AnyVector RightMidFootRotNodeX = ..BodyModel.Interface.Right.MidFootRotNodeX.Pos;
AnyVector RightMidFootRotNodeY = ..BodyModel.Interface.Right.MidFootRotNodeY.Pos;
};
#endif
Import of joint angles (druing motion optimization):
#if LEFT_LEG_TD == 1
AnyKinEqInterPolDriver JntDriverLeftLegTD = {
FileErrorContinueOnOff = On;
Type = Bspline;
BsplineOrder = 4;
FileName = RESULTS_PATH +Main.TrialSpecificData.NameOfFile+"-output-euler-leftlegtd.txt";
AnyKinMeasureOrg &HipFlexion = …BodyModel.Interface.Left.HipFlexion;
AnyKinMeasureOrg &HipAbduction = …BodyModel.Interface.Left.HipAbduction;
AnyKinMeasureOrg &HipExternalRotation = …BodyModel.Interface.Left.HipExternalRotation;
AnyKinMeasureOrg &KneeFlexion = …BodyModel.Interface.Left.KneeFlexion;
AnyKinMeasureOrg &AnklePlantarFlexion = …BodyModel.Interface.Left.AnklePlantarFlexion;
AnyKinMeasureOrg &SubTalarEversion = …BodyModel.Interface.Left.SubTalarEversion;
AnyKinMeasureOrg &LeftMidFootRotNodeZ = …BodyModel.Interface.Left.MidFootRotNodeZ;
AnyKinMeasureOrg &LeftMidFootSupination = …BodyModel.Interface.Left.MidFootRotNodeX;
AnyKinMeasureOrg &LeftMidFootAdduction = …BodyModel.Interface.Left.MidFootRotNodeY;
Reaction.Type={Off,Off,Off,Off,Off,Off,Off,Off,Off};
};
#endif
#if RIGHT_LEG_TD == 1
AnyKinEqInterPolDriver JntDriverRightLegTD = {
FileErrorContinueOnOff = On;
Type = Bspline;
BsplineOrder = 4;
FileName = RESULTS_PATH +Main.TrialSpecificData.NameOfFile+"-output-euler-rightlegtd.txt";
AnyKinMeasureOrg &HipFlexion = …BodyModel.Interface.Right.HipFlexion;
AnyKinMeasureOrg &HipAbduction = …BodyModel.Interface.Right.HipAbduction;
AnyKinMeasureOrg &HipExternalRotation = …BodyModel.Interface.Right.HipExternalRotation;
AnyKinMeasureOrg &KneeFlexion = …BodyModel.Interface.Right.KneeFlexion;
AnyKinMeasureOrg &AnklePlantarFlexion = …BodyModel.Interface.Right.AnklePlantarFlexion;
AnyKinMeasureOrg &SubTalarEversion = …BodyModel.Interface.Right.SubTalarEversion;
AnyKinMeasureOrg &RightMidFootRotNodeZ = …BodyModel.Interface.Right.MidFootRotNodeZ;
AnyKinMeasureOrg &RightMidFootRotNodeX = …BodyModel.Interface.Right.MidFootRotNodeX;
AnyKinMeasureOrg &RightMidFootRotNodeY = …BodyModel.Interface.Right.MidFootRotNodeY;
Reaction.Type={Off,Off,Off,Off,Off,Off,Off,Off,Off};
};
#endif
Joint muscles during inverse dynamics:
#if InverseDynamicModel == 1
// Add Joint muscles to mid foot joint
Mus = {
AnyFolder MtPhal3JointMucles = {
// Muscle properties
AnyMuscleModel mus = {F0 = 50;};
// Combine antagonistic values
AnyVar RotNodeX_Moment = RotNodeX_Pos.Fout[0] + RotNodeX_Neg.Fout[0];
AnyVar RotNodeY_Moment = RotNodeY_Pos.Fout[0] + RotNodeY_Neg.Fout[0];
AnyVar RotNodeZ_Moment = RotNodeZ_Pos.Fout[0] + RotNodeZ_Neg.Fout[0];
// Artificial joint muscles
AnyGeneralMuscle RotNodeX_Pos = {
AnyKinMeasure& org = …Jnt.MtPhal3.RotNodeX;
AnyMuscleModel& mus = .mus;
ForceDirection = 1;
};
AnyGeneralMuscle RotNodeX_Neg = {
AnyKinMeasure& org = …Jnt.MtPhal3.RotNodeX;
AnyMuscleModel& mus = .mus;
ForceDirection = -1;
};
AnyGeneralMuscle RotNodeY_Pos = {
AnyKinMeasure& org = …Jnt.MtPhal3.RotNodeY;
AnyMuscleModel& mus = .mus;
ForceDirection = 1;
};
AnyGeneralMuscle RotNodeY_Neg = {
AnyKinMeasure& org = …Jnt.MtPhal3.RotNodeY;
AnyMuscleModel& mus = .mus;
ForceDirection = -1;
};
AnyGeneralMuscle RotNodeZ_Pos = {
AnyKinMeasure& org = …Jnt.MtPhal3.RotNodeZ;
AnyMuscleModel& mus = .mus;
ForceDirection = 1;
};
AnyGeneralMuscle RotNodeZ_Neg = {
AnyKinMeasure& org = …Jnt.MtPhal3.RotNodeZ;
AnyMuscleModel& mus = .mus;
ForceDirection = -1;
};
}; // end MtPhal3JointMucles
}; // end Mus