519 lines (458 with data), 22.7 kB
/* Class Template OptimizeBVH_Origin
This class template can be useful when BVH recordings consist
of interaction of the subject with environment objects. In such
trials, it can be hard to locate the environment object relative
to the subject. An example could be a subject lifting a box,
where the position of the box is known but the BVH recordings of
different trials results in the subject being located inconsistently
with respect to the box.
This class template optimizes the origin of the BVH model.
Normally, the origin of the BVH recording is coincident with the
Global Ref of the model in AnyMoCap models. The BVH stick figure
and consequently, the movement of the human subject is reconstructed
from the origin of the BVH recording. Whereas environment objects
are normally located with respect to the Global Ref of the model.
The origin of the BVH model (recording) can be set to another position
and orientation with respect to the Global Ref. This class template
can optimize the origin of the BVH model such that a target segment
of the human model (L/R Foot/Hand) is positioned and oriented in a
known way for a known time interval while following the recorded
motion. For example, the class template can set the origin of the BVH
recording such that the right hand is positioned and oriented in a
known way (such as on a box) in a known and definite time interval
of the recording (such as the time interval in which the subject
grasps the box).
The class works by defining three ref nodes on the BVH stick figure
and three corresponding nodes at the target. To help visualization,
a drawing of the selected human segment is generated at the target.
The corresponding nodes are driven to be coincident by using
AnyKinMotion driver. These drivers are active only during the active
time interval. This is implemented by a square wave function that is
set to 1 during the active period and zero outside the active period.
Finally, the origin of the BVH recording is set using a design
variable that is optimized through the default parameter optimization
study of the AnyMoCap model.
Class arguments:
---------------
HUMAN_SEG: <Optional. Default ="RFOOT">
Is the human segment that must be positioned and oriented in the
target postion and orientation.
REPEAT_USE: <Optional. Default = 0>
Switch to identify if the class template is being used more than one
time. For multiple use of the of the class template in the same model,
the switch must be set to 0 only one time and to 1 for other times.
This switch prevents error due to the class template defining the same
objects multiple times.
OPT_LIN_X: <Optional. Default = 1>
Switch to enable optimization of BVH origin in translation in X axis.
OPT_LIN_Y: <Optional. Default = 0>
Switch to enable optimization of BVH origin in translation in Y axis.
OPT_LIN_Z: <Optional. Default = 1>
Switch to enable optimization of BVH origin in translation in Z axis.
OPT_ROT_X: <Optional. Default = 0>
Switch to enable optimization of BVH origin in rotation in X axis.
OPT_ROT_Y: <Optional. Default = 1>
Switch to enable optimization of BVH origin in rotation in Y axis.
OPT_ROT_Z: <Optional. Default = 0>
Switch to enable optimization of BVH origin in rotation in Z axis.
NAME_PARA_OPT: <Optional. Default = BVH_Origin>
Is the name of the parameter that will be included in the parameter
optimization study with suffix for direction (eg: NAME_PARA_OPT_LinX)
REF_FRAME_FOR_TARGET: <Optional. Default = Main.EnvironmentModel.GlobalRef>
Is the reference frame for defining the target position and orientation.
BVH_FILE_DATA: <Optional. Default = Main.ModelSetup.BVHFileData>
Is the pointer to the AnyInputBVH object in the model. This is the class
that reads the BVH file.
HUMAN_MODEL: <Optional. Default = Main.HumanModel>
Is the pointer to the human model in the model.
{
Active_Time_Start: <Obligatory>
Is the start of the time interval where the selected human segment
must be in the target position and orientation. AnyVector
Active_Time_End: <Obligatory>
Is the end of the time interval where the selected human segment
must be in the target position and orientation. AnyVector
Target_Position: <Optional. Default = {0.0, 0.0, 0.0};>
Is the position vector of the Target referred in REF_FRAME_FOR_TARGET.
Target_Orientation: <Optional. Default = {{1,0,0},{0,1,0},{0,0,1}};>
Is the rotational transformation matrix of the Target referred in
REF_FRAME_FOR_TARGET.
Optional initialization:
-----------------------
Settings:
InitialGuess : Initial guess for the BVH Origin in the order: {lin_x,
lin_y, lin_z, rot_x, rot_y, rot_z}. in m or radians.
Weight : Weight of the AnyKinMotion drivers in the active time interval
dt_Ratio : Start and end transition time of the weight function
expressed as fraction of the active time interval. For more info,
please see reference for AnyFunSquareWave
}
Please note:
------------
* When you implement this class in a copy of the BVH Xsens model from the AMMR, it is possible
that the TrialSpecificData.any file defines LoadParametersFrom = {""};. If you want to save the
optimized origin and load it again in the future, please comment the definition of
LoadParametersFrom so that the AnyMoCap model can use the default definition of the filename.
The class template will provide a warning if LoadParametersFrom = {""};
Initialization example:
----------------------
OptimizeBVH_Origin <Object_name> (
HUMAN_SEG = "<*RFOOT|LFOOT|RHAND|LHAND>"
// Optional Initialization showing default values
REPEAT_USE = 0,
OPT_LIN_X = 1, OPT_LIN_Y = 0, OPT_LIN_Z = 1,
OPT_ROT_X = 0, OPT_ROT_Y = 1, OPT_ROT_Z = 0,
NAME_PARA_OPT = BVH_Origin,
PARAMETER_OPT_STUDY = Main.Studies.ParameterIdentification,
REF_FRAME_FOR_TARGET = Main.EnvironmentModel.GlobalRef,
BVH_FILE_DATA = Main.ModelSetup.BVHFileData,
HUMAN_MODEL = Main.HumanModel
) =
{
Active_Time_Start = 0.1; // Obligatory
Active_Time_End = 0.15; // Obligatory
Target_Position = {0.0, 0.0, 0.0};
Target_Orientation = {{1,0,0},{0,1,0},{0,0,1}};
Settings = {
InitialGuess = {0.0,0.0,0.0,0.0,0.0,0.0};
Weight = 1;
dt_Ratio = {0.1,0.1};
};
};
*/
#class_template OptimizeBVH_Origin (
HUMAN_SEG = "RFOOT",
REPEAT_USE = 0,
OPT_LIN_X = 1, OPT_LIN_Y = 0, OPT_LIN_Z = 1,
OPT_ROT_X = 0, OPT_ROT_Y = 1, OPT_ROT_Z = 0,
NAME_PARA_OPT = BVH_Origin,
PARAMETER_OPT_STUDY = Main.Studies.ParameterIdentification,
REF_FRAME_FOR_TARGET = Main.EnvironmentModel.GlobalRef,
BVH_FILE_DATA = Main.ModelSetup.BVHFileData,
HUMAN_MODEL = Main.HumanModel
)
{
/// The start of the time interval where the selected human segment must be in the target position and orientation.
#var AnyVar Active_Time_Start;
/// The end of the time interval where the selected human segment must be in the target position and orientation.
#var AnyVar Active_Time_End;
/// A reference to REF_FRAME_FOR_TARGET
AnyFolder &Ref_Frame_For_Target = REF_FRAME_FOR_TARGET;
/// The position vector of the Target referred in REF_FRAME_FOR_TARGET.
#var AnyVec3 Target_Position = {0.0, 0.0, 0.0};
/// The rotational transformation matrix of the Target referred in REF_FRAME_FOR_TARGET.
#var AnyMat33 Target_Orientation = {{1,0,0},{0,1,0},{0,0,1}};
AnyFolder Settings = {
/// Initial Guess of the BVH Origin in the order: {lin_x, lin_y, lin_z, rot_x, rot_y, rot_z}. in m or radians.
#var AnyFloat InitialGuess = {0.0,0.0,0.0,0.0,0.0,0.0};
/// Weight of the AnyKinMotion drivers during the active time interval
#var AnyVar Weight = 1;
/// Start and end transition time of the weight function expressed as fraction of the active time interval.
/// For more info, please see reference for AnyFunSquareWave
#var AnyFloat dt_Ratio = {0.1,0.1};
};
// Implement code only if HUMAN_SEG is correctly defined, else give appropriate error.
// Error is defined at the end of the class template.
#if (HUMAN_SEG =="LFOOT")|(HUMAN_SEG =="RFOOT")|(HUMAN_SEG =="LHAND")|(HUMAN_SEG =="RHAND")
AnyFolder InputArgs = {
// warning for LoadParametersFrom value. BVH models normally don't have any parameters to be optimized. Thus, the anyset file with
// parameters won't exist and LoadParametersFrom value is defined as {""}; in TrialSpecificData.any to avoid issues with
// RunAnalysis.LoadParameters Operation. The definition of LoadParametersFrom in TrialSpecificData.any must be commented out
// so that AnyBody can define the default value for LoadParametersFrom value. Alternatively, provide the name of the file from
// which parameters should be loaded.
AnyInt Warning_LoadParams = warn(neqfun(Main.ModelSetup.TrialSpecificData.LoadParametersFrom,""), strformat("\n" +
"------------------------------------------------------------------------------------------------------\n"+
"Main.ModelSetup.TrialSpecificData.LoadParametersFrom is defined as empty. Loading parameters operation will not work!\n" +
"Please comment its definition in TrialSpecificData.any to let AnyBody define this value automatically \n" +
"or, provide the name of the file from which parameters should be loaded.\n" +
"------------------------------------------------------------------------------------------------------"));
// Error for ActiveTimeInterval.
AnyInt Error_ActiveTimeInterval = assert(gtfun(.Active_Time_End,.Active_Time_Start), "Please ensure Active_Time_End is greater than Active_Time_Start");
#if HUMAN_SEG == "LFOOT"
// create nodes on the BVH model
AnyRefFrame &BVHSeg = BVH_FILE_DATA.Model.Hips.LeftHip.LeftKnee.LeftAnkle.Seg;
BVHSeg = {
AnyRefNode NAME_PARA_OPT##_Node0 = {
sRel = {0,.LeftToe.sRel[1],0};
AnyDrawRefFrame drw ={};
AnyRefNode Node1 = {
sRel = {-0.02,0,..LeftToe.sRel[2]};
AnyDrawNode drw ={};
};
AnyRefNode Node2 = {
sRel = {0.05,0,..LeftToe.sRel[2]};
AnyDrawNode drw ={};
};
}; // Heel Node
}; // BVHSeg
// Define values for drawing the HUMAN_SEG at the target point and orientation.
TargetFrameRef.NAME_PARA_OPT##_Node0 = {
AnyFileVar STLFilename = HUMAN_MODEL.BodyModel.Left.Leg.Seg.STL.FilenameFoot;
AnyFloat STLPos = -..BVHSeg.NAME_PARA_OPT##_Node0.sRel + {0,HUMAN_MODEL.BodyModel.Left.Leg.Seg.Talus.SubTalarJoint.sRel[1],0};
AnyMat33 STLOrientation = RotMat(-pi/2,y);
AnyFloat STLScale = {1,1,-1};
AnyFunTransform3D &STLTransform = HUMAN_MODEL.BodyModel.Left.Leg.Seg.Foot.Scale;
};
#endif
#if HUMAN_SEG == "RFOOT"
// create nodes on the BVH model
AnyRefFrame &BVHSeg = BVH_FILE_DATA.Model.Hips.RightHip.RightKnee.RightAnkle.Seg;
BVHSeg = {
AnyRefNode NAME_PARA_OPT##_Node0 = {
sRel = {0,.RightToe.sRel[1],0};
AnyDrawRefFrame drw ={};
AnyRefNode Node1 = {
sRel = {0.02,0,..RightToe.sRel[2]};
AnyDrawNode drw ={};
};
AnyRefNode Node2 = {
sRel = {-0.05,0,..RightToe.sRel[2]};
AnyDrawNode drw ={};
};
}; // Heel Node
}; // BVHSeg
// Define values for drawing the HUMAN_SEG at the target point and orientation.
TargetFrameRef.NAME_PARA_OPT##_Node0 = {
AnyFolder &HumRef = HUMAN_MODEL.BodyModel.Right.Leg.Seg;
AnyFileVar STLFilename = HUMAN_MODEL.BodyModel.Right.Leg.Seg.STL.FilenameFoot;
AnyFloat STLPos = -..BVHSeg.NAME_PARA_OPT##_Node0.sRel + {0,HUMAN_MODEL.BodyModel.Right.Leg.Seg.Talus.SubTalarJoint.sRel[1],0};
AnyMat33 STLOrientation = RotMat(-pi/2,y);
AnyFloat STLScale = {1,1,1};
AnyFunTransform3D &STLTransform = HUMAN_MODEL.BodyModel.Right.Leg.Seg.Foot.Scale;
};
#endif
#if HUMAN_SEG == "LHAND"
// create nodes on the BVH model
AnyRefFrame &BVHSeg = BVH_FILE_DATA.Model.Hips.Chest.Chest2.Chest3.Chest4.LeftCollar.LeftShoulder.LeftElbow.LeftWrist.Seg;
BVHSeg = {
AnyRefNode NAME_PARA_OPT##_Node0 = {
sRel = {0,0,0};
AnyDrawRefFrame drw ={};
AnyRefNode Node1 = {
sRel = ..LHT1.sRel;
AnyDrawNode drw ={};
};
AnyRefNode Node2 = {
sRel = ..LHT2.sRel;
AnyDrawNode drw ={};
};
}; // Wrist Node
}; // BVHSeg
// Define values for drawing the HUMAN_SEG at the target point and orientation.
TargetFrameRef.NAME_PARA_OPT##_Node0 = {
AnyFileVar STLFilename = HUMAN_MODEL.BodyModel.Left.ShoulderArm.STL.FileNameHand;
AnyFloat STLPos = -..BVHSeg.NAME_PARA_OPT##_Node0.sRel+HUMAN_MODEL.BodyModel.Left.ShoulderArm.Seg.Hand.Ref.wj.sRel;
AnyMat33 STLOrientation = RotMat(-pi/2,x)*RotMat(-pi,y);
AnyFloat STLScale = {1,1,-1};
AnyFunTransform3D &STLTransform = HUMAN_MODEL.Scaling.GeometricalScaling.Left.Hand.ScaleFunction;
};
#endif
#if HUMAN_SEG == "RHAND"
// create nodes on the BVH model
AnyRefFrame &BVHSeg = BVH_FILE_DATA.Model.Hips.Chest.Chest2.Chest3.Chest4.RightCollar.RightShoulder.RightElbow.RightWrist.Seg;
BVHSeg = {
AnyRefNode NAME_PARA_OPT##_Node0 = {
sRel = {0,0,0};
AnyDrawRefFrame drw ={};
AnyRefNode Node1 = {
sRel = ..RHT1.sRel;
AnyDrawNode drw ={};
};
AnyRefNode Node2 = {
sRel = ..RHT2.sRel;
AnyDrawNode drw ={};
};
}; // Wrist Node
}; // BVHSeg
// Define values for drawing the HUMAN_SEG at the target point and orientation.
TargetFrameRef.NAME_PARA_OPT##_Node0 = {
AnyFileVar STLFilename = HUMAN_MODEL.BodyModel.Right.ShoulderArm.STL.FileNameHand;
AnyFloat STLPos = -..BVHSeg.NAME_PARA_OPT##_Node0.sRel-HUMAN_MODEL.BodyModel.Right.ShoulderArm.Seg.Hand.Ref.wj.sRel;
AnyMat33 STLOrientation = RotMat(-pi/2,x);
AnyFloat STLScale = {1,1,1};
AnyFunTransform3D &STLTransform = HUMAN_MODEL.Scaling.GeometricalScaling.Right.Hand.ScaleFunction;
};
#endif
// Create Target nodes corresponding to the nodes defined on the BVH model
AnyRefFrame &TargetFrameRef = REF_FRAME_FOR_TARGET;
TargetFrameRef = {
AnyRefNode NAME_PARA_OPT##_Node0 = {
sRel = ...Target_Position;
ARel = ...Target_Orientation;
AnyDrawRefFrame drw ={ScaleXYZ = 0.15*{1,1,1}; RGB = {0.65,0.65,0.65};};
// Visualize target human segment
AnyDrawSurf drwSurf = {
FileName = .STLFilename;
RelPos = .STLPos;
RelRotMat = .STLOrientation;
ScaleXYZ = .STLScale;
Opacity = 0.3 + ....Functions.WeightFun(.t)[0]*0.5;
// RGB ={....Functions.WeightFun(.t)[0]*0.5,(....Settings.Weight-....Functions.WeightFun(.t)[0])*0.7,0.7*....Settings.Weight}/....Settings.Weight;
RGB = iffun(gtfun(....Functions.WeightFun(.t)[0],0),{0.2,0.8,0.2},{0.65,0.65,0.65});
AnyFunTransform3D &Scale = .STLTransform;
};
};
AnyRefNode NAME_PARA_OPT##_Node1 = {
sRel = ..BVHSeg.NAME_PARA_OPT##_Node0.Node1.sRel*...Target_Orientation'+...Target_Position;
AnyDrawNode drw ={ScaleXYZ = 0.015*{1,1,1}; RGB = {0.65,0.65,0.65};Opacity = 0.5;};
};
AnyRefNode NAME_PARA_OPT##_Node2 = {
sRel = ..BVHSeg.NAME_PARA_OPT##_Node0.Node2.sRel*...Target_Orientation'+...Target_Position;
AnyDrawNode drw ={ScaleXYZ = 0.015*{1,1,1}; RGB = {0.65,0.65,0.65};Opacity = 0.5;};
};
}; // TargetFrameRef
}; // InputArgs
// Define functions needed for the parameter optimization.
AnyFolder Functions = {
// WeightFun makes a square wave function to consider the active time interval.
// WeightFun is equal to the weight value in the active time interval and zero
// outside the active time interval
AnyFunSquareWave WeightFun =
{
InitialValues = {0.0} + 0*..InputArgs.Error_ActiveTimeInterval; // Enforce Active Time Interval error
Ts = {..Active_Time_Start,..Active_Time_End};
Values = {{..Settings.Weight,0}};
dT = ..Settings.dt_Ratio*(Ts[1]-Ts[0]);
};
// Create constant interpol functions that will be used to drive the linear measure between
// BVH model nodes and target nodes to zero.
AnyFunInterpol DriverPosNode0 = {
Type = ConstantValue;
T = BVH_FILE_DATA.Data.Abscissa.Sample*BVH_FILE_DATA.Header.FrameTime;
Data = {repmat(SizesOf(T)[0], 0),
repmat(SizesOf(T)[0], 0),
repmat(SizesOf(T)[0], 0)};
};
AnyFunInterpol DriverPosNode1 = {
Type = ConstantValue;
T = BVH_FILE_DATA.Data.Abscissa.Sample*BVH_FILE_DATA.Header.FrameTime;
Data = {repmat(SizesOf(T)[0], 0),
repmat(SizesOf(T)[0], 0),
repmat(SizesOf(T)[0], 0)};
};
AnyFunInterpol DriverPosNode2 = {
Type = ConstantValue;
T = BVH_FILE_DATA.Data.Abscissa.Sample*BVH_FILE_DATA.Header.FrameTime;
Data = {repmat(SizesOf(T)[0], 0),
repmat(SizesOf(T)[0], 0),
repmat(SizesOf(T)[0], 0)};
};
};// Functions
// Define a variable that will be used to set the Origin and Axes value of the BVH origin
AnyFolder BVHOrigin = {
AnyFloat NAME_PARA_OPT = .Settings.InitialGuess;
AnyFixedRefFrame &BVHOriginRef = BVH_FILE_DATA.Model.GlobalRef;
BVHOriginRef = {
// Set origin and axes only for the primary use of class template.
#if REPEAT_USE == 0
Origin = {.NAME_PARA_OPT[0], .NAME_PARA_OPT[1], .NAME_PARA_OPT[2]};
Axes = RotMat(.NAME_PARA_OPT[3],x)
*RotMat(.NAME_PARA_OPT[4],y)
*RotMat(.NAME_PARA_OPT[5],z);
#else
#endif
};
}; // BVH Origin
// Insert conditionally the parameter used to set the BVH origin in the existing parameter optimization study
// and create drivers between BVH model nodes and target nodes.
AnyFolder &ParaOptStudy = PARAMETER_OPT_STUDY;
ParaOptStudy = {
// Insert design variables only in case of primary use of the class template.
#if REPEAT_USE == 0
// and only if they are required to be optimized.
#if OPT_LIN_X == 1
AnyDesVar NAME_PARA_OPT##_LinX = {
Val = ..BVHOrigin.NAME_PARA_OPT[0];
};
#endif
#if OPT_LIN_Y == 1
AnyDesVar NAME_PARA_OPT##_LinY = {
Val = ..BVHOrigin.NAME_PARA_OPT[1];
};
#endif
#if OPT_LIN_Z == 1
AnyDesVar NAME_PARA_OPT##_LinZ = {
Val = ..BVHOrigin.NAME_PARA_OPT[2];
};
#endif
#if OPT_ROT_X == 1
AnyDesVar NAME_PARA_OPT##_RotX = {
Val = ..BVHOrigin.NAME_PARA_OPT[3];
};
#endif
#if OPT_ROT_Y == 1
AnyDesVar NAME_PARA_OPT##_RotY = {
Val = ..BVHOrigin.NAME_PARA_OPT[4];
};
#endif
#if OPT_ROT_Z == 1
AnyDesVar NAME_PARA_OPT##_RotZ = {
Val = ..BVHOrigin.NAME_PARA_OPT[5];
};
#endif
#else
#endif
KinematicStudyForParameterIdentification = {
#if REPEAT_USE == 0
// include BVH model in the optimization study (only for primary use)
AnyFolder &mocapmodel = BVH_FILE_DATA;
#else
#endif
// Create Drivers between BVH model nodes and target nodes. The drivers are
// created inside a folder that can be uniquely named using NAME_PARA_OPT
AnyFolder NAME_PARA_OPT##_OptDrivers = {
AnyKinMotion Node0 = {
viewKinMeasure.Size = 0.2;
WeightFun = {&....Functions.WeightFun,
&....Functions.WeightFun,
&....Functions.WeightFun};
AnyKinLinear Lin = {
AnyRefFrame &BVHPoint = .....InputArgs.BVHSeg.NAME_PARA_OPT##_Node0;
AnyRefFrame &TargetPoint = .....InputArgs.TargetFrameRef.NAME_PARA_OPT##_Node0;
};
AnyParamFun &target = ....Functions.DriverPosNode0;
};
AnyKinMotion Node1 = {
viewKinMeasure.Size = 0.2;
WeightFun = {&....Functions.WeightFun,
&....Functions.WeightFun,
&....Functions.WeightFun};
AnyKinLinear Lin = {
AnyRefFrame &BVHPoint = .....InputArgs.BVHSeg.NAME_PARA_OPT##_Node0.Node1;
AnyRefFrame &TargetPoint = .....InputArgs.TargetFrameRef.NAME_PARA_OPT##_Node1;
};
AnyParamFun &target = ....Functions.DriverPosNode1;
};
AnyKinMotion Node2 = {
viewKinMeasure.Size = 0.2;
WeightFun = {&....Functions.WeightFun,
&....Functions.WeightFun,
&....Functions.WeightFun};
AnyKinLinear Lin = {
AnyRefFrame &BVHPoint = .....InputArgs.BVHSeg.NAME_PARA_OPT##_Node0.Node2;
AnyRefFrame &TargetPoint = .....InputArgs.TargetFrameRef.NAME_PARA_OPT##_Node2;
};
AnyParamFun &target = ....Functions.DriverPosNode2;
};
}; // OptDriver
};//KinStudyForParaIden
}; // ParaOptStudy
// Define a sequence of operations to run the the parameter study, update and save values.
#if REPEAT_USE == 0
Main = {
AnyOperationSequence RunBVH_Origin_Optimization =
{
AnyOperation& ParameterOptimization = PARAMETER_OPT_STUDY.ParameterOptimization;
AnyOperationMacro UpdateValues = {
MacroStr = {"classoperation Main " + strquote("Update Values")};
};
AnyOperation& SaveParameters = Main.ModelSetup.Macros.Save_parameters;
};
};
#else
#endif
// Error message for wrong choice of HUMAN_SEG
#else
AnyInt Error_HumanSeg = assert(0,strformat("Please check choice of HUMAN_SEG. Only one of \x22LFOOT\x22, \x22RFOOT\x22, \x22LHAND\x22, or \x22RHAND\x22 is allowed."));
#endif
}; // class template
Datasets
Models
