/// <summary>
/// Dynamically estimates the target transform
/// </summary>
/// <returns></returns>
private MTransform ComputeTargetTransform(HandContainer hand)
{
MTransform target = new MTransform()
{
ID = "target"
};
//Use the constraint (if defined)
if (hand.Instruction.Constraints != null && hand.Instruction.Constraints.Exists(s => s.ID == hand.Instruction.Properties["TargetID"]))
{
MConstraint match = hand.Instruction.Constraints.Find(s => s.ID == hand.Instruction.Properties["TargetID"]);
//Compute the global position and rotation of the geometry constraint
target.Position = match.GeometryConstraint.GetGlobalPosition(this.SceneAccess);
target.Rotation = match.GeometryConstraint.GetGlobalRotation(this.SceneAccess);
}
//Gather from the scene
else
{
target = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["TargetID"]);
}
return(target);
}
/// <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);
}
/// <summary>
/// Basic to step routine which computes the result of the current frame
/// </summary>
/// <param name="time"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
DrawingCalls = new List <MDrawingCall>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current,
Constraints = simulationState.Constraints ?? new List <MConstraint>()
};
//The presently active constraints
List <MConstraint> globalConstraints = new List <MConstraint>(result.Constraints);
//The local constraints defined within the MMU
List <MConstraint> localConstraints = new List <MConstraint>();
//Setup the constraint manager and use the local constraints
this.constraintManager.SetConstraints(ref localConstraints);
//Handle each active hand
for (int i = this.activeHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.activeHands[i];
//Skip if hand is not initialized
if (!hand.Initialized)
{
continue;
}
//Get the transform of the object to be positioned
MTransform currentObjectTransform = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["SubjectID"]);
//Get the transform of the target
MTransform targetObjectTransform = null;
//Determine the next location of the object (at the end of the frame)
MTransform nextObjectTransform = null;
//Check if trajectory is defined
if (hand.Trajectory != null)
{
//The last point is the target transform
targetObjectTransform = hand.Trajectory.Last();
//The current rajectory point
MTransform currentTrajectoryPoint = hand.Trajectory[hand.TrajectoryIndex];
//Estimate the next transfom based on local motion planning
nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, currentTrajectoryPoint.Position, currentTrajectoryPoint.Rotation, hand.CollisionAvoidance);
//Get the current distance
float currentDistance = nextObjectTransform.Position.Subtract(hand.Trajectory[hand.TrajectoryIndex].Position).Magnitude();
float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextObjectTransform.Rotation, hand.Trajectory[hand.TrajectoryIndex].Rotation);
//Check if close to current target -> move to next target
if (currentDistance < this.translationThreshold && currentAngularDistance < this.rotationThreshold && hand.TrajectoryIndex < hand.Trajectory.Count - 1)
{
hand.TrajectoryIndex++;
}
}
//Default behavior if no trajectory is specified
else
{
targetObjectTransform = this.ComputeTargetTransform(hand);
//Estimate the next pose of the scene object
nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, targetObjectTransform.Position, targetObjectTransform.Rotation, hand.CollisionAvoidance);
}
//Set the pose of the object to the next estimated pose
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = hand.Instruction.Properties.GetValue("SubjectID", "subjectID"),
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
//Compute the next handpose based on the offset
MTransform nextHandTransform = new MTransform("", nextObjectTransform.TransformPoint(hand.Offset.Position), nextObjectTransform.TransformRotation(hand.Offset.Rotation));
//Set the ik constraints
constraintManager.SetEndeffectorConstraint(hand.Type, nextHandTransform.Position, nextHandTransform.Rotation);
//To do add constraints
float distance = (nextObjectTransform.Position.Subtract(targetObjectTransform.Position)).Magnitude();
float angularDistance = (float)MQuaternionExtensions.Angle(nextObjectTransform.Rotation, targetObjectTransform.Rotation);
//Check if goal criteria fulfilled
if (distance < this.translationThreshold && angularDistance < this.rotationThreshold)
{
//Increment the time
hand.ElapsedHoldTime += time;
if (hand.ElapsedHoldTime < hand.HoldTime)
{
continue;
}
this.activeHands.RemoveAt(i);
//Add new finished event
if (hand.BothHanded)
{
if (activeHands.Count == 0)
{
result.Events.Add(new MSimulationEvent(hand.Instruction.Name, mmiConstants.MSimulationEvent_End, hand.Instruction.ID));
}
}
//Single handed grasp
else
{
result.Events.Add(new MSimulationEvent(hand.Instruction.Name, mmiConstants.MSimulationEvent_End, hand.Instruction.ID));
}
}
}
//Get the properties from the constraint manager
List <MConstraint> jointConstraints = this.constraintManager.GetJointConstraints();
//Use the ik service if at least one constraint must be solved
if (jointConstraints.Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, jointConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
//Configure the constraint manager to operate on the global constraints
constraintManager.SetConstraints(ref globalConstraints);
//Combine the global with the local ones
constraintManager.Combine(localConstraints);
//Provide the combined constraints as result
result.Constraints = globalConstraints;
//Return the simulation result
return(result);
}
