/// <summary>
/// Returns a list of MTRansform
/// </summary>
/// <param name="pathConstraint"></param>
/// <returns></returns>
public static List <MTransform> GetMTransformList(this MPathConstraint pathConstraint)
{
//Create a list for storing the full trajectory
List <MTransform> list = new List <MTransform>();
foreach (MGeometryConstraint mg in pathConstraint.PolygonPoints)
{
MTransform t = null;
if (mg.ParentToConstraint != null)
{
t = new MTransform
{
ID = "",
Position = mg.ParentToConstraint.Position,
Rotation = mg.ParentToConstraint.Rotation
};
}
else
{
t = new MTransform
{
ID = "",
Position = mg.TranslationConstraint.GetVector3(),
Rotation = MQuaternionExtensions.FromEuler(mg.TranslationConstraint.GetVector3())
};
}
list.Add(t);
}
return(list);
}
/// <summary>
/// Maps the path constraint to list of MVector2
/// </summary>
/// <param name="pathConstraint"></param>
/// <returns></returns>
public static List <Vector2> GetVector2List(this MPathConstraint pathConstraint)
{
//Create a list for storing the full trajectory
List <Vector2> list = new List <Vector2>();
foreach (MGeometryConstraint mg in pathConstraint.PolygonPoints)
{
list.Add(new Vector2((float)mg.ParentToConstraint.Position.X, (float)mg.ParentToConstraint.Position.Z));
}
return(list);
}
/// <summary>
/// Maps the path constraint to list of MVector2
/// </summary>
/// <param name="pathConstraint"></param>
/// <returns></returns>
public static List <MVector3> GetMVector3List(this MPathConstraint pathConstraint)
{
//Create a list for storing the full trajectory
List <MVector3> list = new List <MVector3>();
foreach (MGeometryConstraint mg in pathConstraint.PolygonPoints)
{
list.Add(mg.TranslationConstraint.GetVector3());
}
return(list);
}
/// <summary>
/// Returns a trajectory constraint describing the 2D positions (x-z)
/// </summary>
/// <returns></returns>
public MConstraint GetPathConstraint()
{
MConstraint constraint = new MConstraint(System.Guid.NewGuid().ToString());
MPathConstraint pathConstraint = new MPathConstraint()
{
PolygonPoints = new List <MGeometryConstraint>()
};
foreach (Transform t in this.Points)
{
pathConstraint.PolygonPoints.Add(new MGeometryConstraint()
{
ParentObjectID = "",
ParentToConstraint = new MTransform("", new MVector3(t.position.x, 0, t.position.z), new MQuaternion(0, 0, 0, 1))
});
}
constraint.PathConstraint = pathConstraint;
return(constraint);
}
/// <summary>
/// Methods parses the parameters defined in the MInstruction
/// </summary>
/// <param name="instruction"></param>
/// <returns></returns>
private MBoolResponse ParseParameters(MInstruction instruction)
{
//Extract the single shot ik parameter if defined
if (instruction.Properties.ContainsKey("SingleShotIK"))
{
bool.TryParse(instruction.Properties["SingleShotIK"], out singleShotIK);
}
//Extract the velocity if defined
if (instruction.Properties.ContainsKey("Velocity"))
{
this.velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture);
}
//Extract the angular velocity
if (instruction.Properties.ContainsKey("AngularVelocity"))
{
this.angularVelocity = float.Parse(instruction.Properties["AngularVelocity"], System.Globalization.CultureInfo.InvariantCulture);
}
//Extract the information with regard to the target
if (instruction.Properties.ContainsKey("TargetID"))
{
//Use the constraint (if defined)
if (instruction.Constraints != null && instruction.Constraints.Exists(s => s.ID == instruction.Properties["TargetID"]))
{
MConstraint match = instruction.Constraints.Find(s => s.ID == instruction.Properties["TargetID"]);
this.targetTransform = new MTransform()
{
ID = "target",
Position = match.GeometryConstraint.GetGlobalPosition(this.SceneAccess),
Rotation = match.GeometryConstraint.GetGlobalRotation(this.SceneAccess)
};
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_DEBUG, "Using MGeometryConstraint for target definition");
}
//Gather from the scene
else
{
this.targetTransform = this.SceneAccess.GetTransformByID(instruction.Properties["TargetID"]);
}
}
//Error id not available
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter Target ID not defined"
}
});
}
//Get the target hand
if (instruction.Properties.ContainsKey("Hand"))
{
if (instruction.Properties["Hand"] == "Left")
{
this.handJoint = MJointType.LeftWrist;
}
if (instruction.Properties["Hand"] == "Right")
{
this.handJoint = MJointType.RightWrist;
}
}
//Error target hand not specified
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter hand not defined"
}
});
}
//First extract all parameters
if (instruction.Properties.ContainsKey("Trajectory"))
{
string pathConstraintID = instruction.Properties["Trajectory"];
if (instruction.Constraints != null || instruction.Constraints.Where(s => s.PathConstraint != null && s.ID == pathConstraintID).Count() == 0)
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.PathConstraint != null && s.ID == pathConstraintID).PathConstraint;
//Extract the trajectory
this.trajectory = pathConstraint.GetMTransformList();
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Assigned hand trajectory. Number elements: {trajectory.Count}, hand: {this.handJoint}");
}
else
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"Cannot assign trajectory of hand: {this.handJoint}. No suitable MPathConstraint available.");
}
}
return(new MBoolResponse(true));
}
/// <summary>
/// Method parses the parameters
/// </summary>
/// <param name="instruction"></param>
/// <returns></returns>
private MBoolResponse ParseParameters(MInstruction instruction)
{
MBoolResponse response = new MBoolResponse(true);
//Get the target hand
if (instruction.Properties.ContainsKey("Hand"))
{
if (instruction.Properties["Hand"] == "Left")
{
this.handJoint = MJointType.LeftWrist;
}
if (instruction.Properties["Hand"] == "Right")
{
this.handJoint = MJointType.RightWrist;
}
}
//Error target hand not specified
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter hand not defined"
}
});
}
if (instruction.Properties.ContainsKey("Velocity"))
{
this.velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("AngularVelocity"))
{
this.angularVelocity = float.Parse(instruction.Properties["AngularVelocity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("UseBlending"))
{
bool.TryParse("UseBlending", out bool useBlending);
this.useIK = !useBlending;
}
if (instruction.Properties.ContainsKey("EndBlendDuration"))
{
this.endBlendDuration = float.Parse(instruction.Properties["EndBlendDuration"], System.Globalization.CultureInfo.InvariantCulture);
}
//First extract all parameters
if (instruction.Properties.ContainsKey("Trajectory"))
{
string pathConstraintID = instruction.Properties["Trajectory"];
if (instruction.Constraints != null || instruction.Constraints.Where(s => s.PathConstraint != null && s.ID == pathConstraintID).Count() == 0)
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.PathConstraint != null && s.ID == pathConstraintID).PathConstraint;
//Extract the trajectory
this.trajectory = pathConstraint.GetMTransformList();
//Add the target transform
this.trajectory.Add(new MTransform("targetTransform", new MVector3(0, 0, 0), new MQuaternion(0, 0, 0, 1)));
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Assigned hand trajectory. Number elements: {trajectory.Count}, hand: {this.handJoint}");
}
else
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"Cannot assign trajectory of hand: {this.handJoint}. No suitable MPathConstraint available.");
}
}
return(response);
}
private void OnGUI()
{
if (GUI.Button(new Rect(300, 200, 200, 100), "Grasp object (trajectory)"))
{
MPathConstraint pathConstraint = new MPathConstraint()
{
PolygonPoints = new List <MGeometryConstraint>()
};
foreach (Transform transform in this.TrajectoryPoints)
{
MVector3 position = transform.position.ToMVector3();
MVector3 rotationEuler = transform.rotation.eulerAngles.ToMVector3();
pathConstraint.PolygonPoints.Add(new MGeometryConstraint("")
{
TranslationConstraint = new MTranslationConstraint()
{
Type = MTranslationConstraintType.BOX,
Limits = position.ToMInterval3(this.TranslationTolerance)
},
RotationConstraint = new MRotationConstraint()
{
Limits = rotationEuler.ToMInterval3(this.RotationTolerance)
}
});
}
MConstraint moveConstraint = new MConstraint(System.Guid.NewGuid().ToString())
{
PathConstraint = pathConstraint
};
MInstruction idleInstruction = new MInstruction(MInstructionFactory.GenerateID(), "Idle", "idle");
MInstruction reachRight = new MInstruction(MInstructionFactory.GenerateID(), "reach right", "Pose/Reach")
{
Properties = PropertiesCreator.Create("TargetID", UnitySceneAccess.Instance["GraspTargetR"].ID, "Hand", "Right", "MinDistance", 2.0.ToString()),
};
MInstruction moveRight = new MInstruction(MInstructionFactory.GenerateID(), "move right", "Object/Move")
{
Properties = new Dictionary <string, string>()
{
{ "SubjectID", this.GetComponent <MMISceneObject>().MSceneObject.ID },
{ "Hand", "Right" },
{ "MinDistance", 2.0.ToString() },
{ "trajectory", moveConstraint.ID }
},
StartCondition = reachRight.ID + ":" + mmiConstants.MSimulationEvent_End,
Constraints = new List <MConstraint>()
{
moveConstraint
}
};
MMIAvatar avatar = GameObject.FindObjectOfType <MMIAvatar>();
//this.CoSimulator.Abort();
avatar.CoSimulator.AssignInstruction(idleInstruction, new MSimulationState()
{
Initial = avatar.GetPosture(), Current = avatar.GetPosture()
});
avatar.CoSimulator.AssignInstruction(reachRight, new MSimulationState()
{
Initial = avatar.GetPosture(), Current = avatar.GetPosture()
});
avatar.CoSimulator.AssignInstruction(moveRight, new MSimulationState()
{
Initial = avatar.GetPosture(), Current = avatar.GetPosture()
});
}
}
/// <summary>
/// Computes a path given a start and end position
/// </summary>
/// <param name="from"></param>
/// <param name="target"></param>
/// <param name="filterSceneObjects"></param>
/// <returns></returns>
private MotionTrajectory2D ComputePath(Vector2 from, Vector2 target, bool filterSceneObjects = true)
{
List <MTransform> computedPath = new List <MTransform>();
//Only plan path if distance is above threshold
if ((from - target).magnitude > 0.2f)
{
try
{
//Get all scene objects from the scene
List <MSceneObject> sceneObjects = this.SceneAccess.GetSceneObjects();
///Remove scene objects in range if filtering is enabled
if (filterSceneObjects)
{
MVector3 hipPosition = this.GetGlobalPosition(this.simulationState.Initial, MJointType.PelvisCentre);
MVector3 leftHandPosition = this.GetGlobalPosition(this.simulationState.Initial, MJointType.LeftWrist);
MVector3 rightHandPosition = this.GetGlobalPosition(this.simulationState.Initial, MJointType.RightWrist);
for (int i = sceneObjects.Count - 1; i >= 0; i--)
{
MVector3 sceneObjectPosition = sceneObjects[i].Transform.Position;
float hipDist = (sceneObjectPosition.Subtract(hipPosition)).Magnitude();
float lhandDist = (leftHandPosition.Subtract(sceneObjectPosition)).Magnitude();
float rhandDist = (rightHandPosition.Subtract(sceneObjectPosition)).Magnitude();
if (lhandDist < 0.5f || rhandDist < 0.5f)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_DEBUG, $"Removing scene object {sceneObjects[i].Name}, that is not included in path planning.");
sceneObjects.RemoveAt(i);
}
}
}
//Compute the path using the path planning service
MPathConstraint result = ServiceAccess.PathPlanningService.ComputePath(
new MVector()
{
Values = new List <double>()
{
from.x, from.y
}
},
new MVector()
{
Values = new List <double>()
{
target.x, target.y
}
},
sceneObjects,
new Dictionary <string, string>()
{
{ "mode", "2D" },
{ "time", Serialization.ToJsonString(1.0f) },
{ "radius", Serialization.ToJsonString(0.3f) },
{ "height", Serialization.ToJsonString(0.5f) },
});
//Get the computed path
if (result.PolygonPoints.Count > 0)
{
if (result.PolygonPoints[0].ParentToConstraint != null)
{
computedPath = result.PolygonPoints.Select(s => new MTransform()
{
Position = new MVector3(s.ParentToConstraint.Position.X, 0, s.ParentToConstraint.Position.Z)
}).ToList();
}
else
{
// TODO: Legacy support. Remove in a future version
computedPath = result.PolygonPoints.Select(s => new MTransform()
{
Position = new MVector3(s.TranslationConstraint.X(), 0, s.TranslationConstraint.Z())
}).ToList();
}
}
}
catch (Exception e)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, "Problem at computing path using service " + e.Message + " " + e.StackTrace);
//In case of an exception return the straight line
//To do use an optional flag to adjust the desired behavior, e.g. should an error be returned instead
if (this.useStraightLineIfNoPath)
{
computedPath = new List <MTransform>()
{
new MTransform()
{
Position = new MVector3(from.x, 0, from.y)
},
new MTransform()
{
Position = new MVector3((from.x + target.x) / 2, 0, (from.y + target.y) / 2)
},
new MTransform()
{
Position = new MVector3(target.x, 0, target.y)
},
};
}
}
finally
{
if (this.useStraightLineIfNoPath && computedPath.Count == 0)
{
computedPath = new List <MTransform>()
{
new MTransform()
{
Position = new MVector3(from.x, 0, from.y)
},
new MTransform()
{
Position = new MVector3((from.x + target.x) / 2, 0, (from.y + target.y) / 2)
},
new MTransform()
{
Position = new MVector3(target.x, 0, target.y)
},
};
}
}
}
//If really close to goal -> No detailed planning is required
else
{
computedPath = new List <MTransform>()
{
new MTransform()
{
Position = new MVector3(from.x, 0, from.y)
},
new MTransform()
{
Position = new MVector3((from.x + target.x) / 2, 0, (from.y + target.y) / 2)
},
new MTransform()
{
Position = new MVector3(target.x, 0, target.y)
},
};
}
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_DEBUG, "Computed path elements: " + computedPath.Count);
if (computedPath.Count == 0)
{
return(null);
}
//Create a motion trajectory from the path
return(new MotionTrajectory2D(computedPath, this.Velocity));
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Create a response
MBoolResponse response = new MBoolResponse(true);
//Set default values
this.abort = false;
this.filterSceneObjects = true;
//Assign the simulation state
this.simulationState = simulationState;
//Assign the instruction
this.instruction = instruction;
//Reset the elapsed time
this.elapsedTime = TimeSpan.Zero;
///Parse the properties
bool requiredPropertiesSet = this.ParseProperties(instruction);
//Check if all required properties have been set
if (!requiredPropertiesSet)
{
response.LogData = new List <string>()
{
"Properties are not defined -> cannot start the MMU"
};
response.Successful = false;
return(response);
}
//Get the target transform
MTransform targetTransform = this.GetTarget();
if (targetTransform == null)
{
response.Successful = false;
response.LogData = new List <string>()
{
"Problem at fetching the target transform!"
};
return(response);
}
//Flag indicates wheather a predefined trajectory should be used
bool usePredefinedTrajectory = false;
//Execute instructions on main thread
this.ExecuteOnMainThread(() =>
{
//Set the channel data of the current simulation state
this.SkeletonAccess.SetChannelData(this.simulationState.Current);
//Get the root position and rotation
MVector3 rootPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
MQuaternion rootRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
//Extract the start position
Vector2 startPosition = new Vector2((float)rootPosition.X, (float)rootPosition.Z);
//Get the goal position
this.goalPosition = new Vector2((float)targetTransform.Position.X, (float)targetTransform.Position.Z);
this.lastGoalPosition = this.goalPosition;
//Fetch the trajectory if available
if (instruction.Properties.ContainsKey("Trajectory"))
{
try
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.ID == instruction.Properties["Trajectory"]).PathConstraint;
//Get the actual trajectory from the path constraint
List <Vector2> pointList = pathConstraint.GetVector2List();
//Estimate the distance between the start point and first point of trajectory
float distance = (startPosition - pointList[0]).magnitude;
//If distance to first point of trajectory is below threshold -> Directly connect the start point with the first point of the trajectory
if (distance < 0.5)
{
pointList.Insert(0, startPosition);
trajectory = new MotionTrajectory2D(pointList, this.Velocity);
}
else
{
//Compute a path to the trajectory start location
this.trajectory = this.ComputePath(startPosition, pointList[0], this.filterSceneObjects);
//Add the defined trajectory
this.trajectory.Append(pointList);
//Finally estimate the timestamps based on the velocity
this.trajectory.EstimateTimestamps(this.Velocity);
}
//Set flag that no more planning is required
usePredefinedTrajectory = true;
}
catch (Exception e)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, "UnityLocomotionMMU: Cannot parse trajectory -> plan new one: " + e.Message + " " + e.StackTrace);
}
}
//Only plan the path if no predefined trajectory should be used
if (!usePredefinedTrajectory)
{
//Compute a path which considers the indivudal path goals and the constraints
this.trajectory = this.ComputePath(startPosition, this.goalPosition, filterSceneObjects);
}
if (this.trajectory == null || this.trajectory.Poses.Count == 0)
{
this.abort = true;
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"No path found in assign instruction. Aborting MMU.");
response.LogData = new List <string>()
{
"No path found"
};
response.Successful = false;
}
//Valid path found
if (this.trajectory != null && this.trajectory.Poses.Count > 0)
{
//Create the visualization data for the trajectory (drawing calls)
this.CreateTrajectoryVisualization();
//Set the speed of the animator
this.animator.speed = this.Velocity / 2.0f;
//Reset the goal direction
this.goalPosition = new Vector2();
//Update the animation
this.animator.SetFloat("Velocity", 0);
this.animator.SetFloat("Direction", 0.5f);
//Reset the current index
this.currentIndex = 0;
//Set the internal fields
this.transform.position = new Vector3((float)rootPosition.X, 0, (float)rootPosition.Z);
// In the new intermediate skeleton definition, x is pointing backwards, y up, z right.
this.transform.rotation = Quaternion.Euler(0, new Quaternion((float)rootRotation.X, (float)rootRotation.Y, (float)rootRotation.Z, (float)rootRotation.W).eulerAngles.y - 90.0f, 0);
//Get the discrete poses representing the trajectory
this.discretePoses = this.trajectory.SampleFrames(60);
//Set state to starting
this.state = WalkState.Starting;
//Set flag which indicates the start
this.firstFrame = true;
//Reset the foot trackers
this.leftFootAnimationTracker.Reset();
this.rightFootAnimationTracker.Reset();
}
});
return(response);
}
/// <summary>
/// Method is used to setup the parameters of one individual hand
/// </summary>
/// <param name="type"></param>
/// <param name="instruction"></param>
private void SetupHand(MJointType type, MInstruction instruction)
{
HandContainer hand = this.activeHands.Find(s => s.Type == type);
if (hand != null)
{
this.activeHands.Remove(hand);
}
//Create a new hand
hand = new HandContainer(type, instruction, true);
if (instruction.Properties.ContainsKey("Velocity"))
{
hand.Velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("AngularVelocity"))
{
hand.AngularVelocity = float.Parse(instruction.Properties["AngularVelocity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("Acceleration"))
{
hand.Acceleration = float.Parse(instruction.Properties["Acceleration"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("HoldDuration"))
{
hand.HoldTime = float.Parse(instruction.Properties["HoldDuration"], System.Globalization.CultureInfo.InvariantCulture);
}
if (instruction.Properties.ContainsKey("CollisionAvoidance"))
{
hand.CollisionAvoidance = bool.Parse(instruction.Properties["CollisionAvoidance"]);
if (hand.CollisionAvoidance)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Using local collision avoidance, hand: {hand.Type}");
}
}
//First extract all parameters
if (instruction.Properties.ContainsKey("Trajectory"))
{
string pathConstraintID = instruction.Properties["Trajectory"];
if (instruction.Constraints != null || instruction.Constraints.Where(s => s.PathConstraint != null && s.ID == pathConstraintID).Count() == 0)
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.PathConstraint != null && s.ID == pathConstraintID).PathConstraint;
//Extract the trajectory
hand.Trajectory = pathConstraint.GetMTransformList();
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Assigned hand trajectory. Number elements: {hand.Trajectory.Count}, hand: {hand.Type}");
}
else
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"Cannot assign trajectory of hand: {hand.Type}. No suitable MPathConstraint available.");
}
}
this.activeHands.Add(hand);
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Reset all values
this.hasTrajectory = false;
// Initialize IK Service
this.ServiceAccess.IKService.Setup(this.AvatarDescription, new Dictionary <string, string>());
//Assign the instruction
this.instruction = instruction;
bool hasTarget = false;
String targetID;
//Get the target id using all synonyms
if (instruction.Properties.GetValue(out targetID, "targetID", "TargetID", "objectID"))
{
this.targetObjectTransform = this.SceneAccess.GetTransformByID(targetID);
hasTarget = true;
}
//Error id not available
if (!hasTarget)
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter Target ID not defined"
}
});
}
bool hasSubject = false;
String subjectID;
//Get the subject id using all synonyms
if (instruction.Properties.GetValue(out subjectID, "subjectID", "SubjectID"))
{
this.subjectTransform = this.SceneAccess.GetTransformByID(subjectID);
hasSubject = true;
}
//Error id not available
if (!hasSubject)
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter Target ID not defined"
}
});
}
//Get the target hand
if (instruction.Properties.ContainsKey("Hand"))
{
if (instruction.Properties["Hand"] == "Left")
{
this.handJoint = MJointType.LeftWrist;
}
if (instruction.Properties["Hand"] == "Right")
{
this.handJoint = MJointType.RightWrist;
}
}
//Error target hand not specified
else
{
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Required parameter hand not defined"
}
});
}
//Handle the trajectory constraint (if defined)
if (instruction.Properties.ContainsKey("trajectory"))
{
string trajectoryID = instruction.Properties["trajectory"];
if (instruction.Constraints != null)
{
MConstraint constraint = instruction.Constraints.Find(s => s.ID == trajectoryID);
if (constraint.PathConstraint != null)
{
this.trajectory = constraint.PathConstraint;
this.hasTrajectory = true;
}
}
}
//Compute the relative position and rotation
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(simulationState.Current.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(simulationState.Current.AvatarID, this.handJoint);
/* Old system could access joint positions from MAvatarPosture directly, now we have to use the intermediate skeleton.
* MVector3 currentHandPosition = simulationState.Initial.GetGlobalPosition(this.handJoint);
* MQuaternion currentHandRotation = simulationState.Initial.GetGlobalRotation(this.handJoint);
*/
//Compute the offsets between hand <-> object
this.handPositionOffset = this.subjectTransform.InverseTransformPoint(currentHandPosition);
this.handRotationOffset = this.subjectTransform.InverseTransformRotation(currentHandRotation);
return(new MBoolResponse(true));
}