/// <summary>
/// Returns the transform of the target
/// </summary>
/// <returns></returns>
private MTransform GetTarget()
{
if (instruction.Properties.ContainsKey("TargetID"))
{
//First check if this is a gemoetry constraint
if (instruction.Constraints != null && instruction.Constraints.Exists(s => s.ID == instruction.Properties["TargetID"]))
{
MConstraint match = instruction.Constraints.Find(s => s.ID == instruction.Properties["TargetID"]);
return(match.GeometryConstraint.ParentToConstraint);
}
else
{
return(this.SceneAccess.GetTransformByID(instruction.Properties["TargetID"]));
}
}
if (instruction.Properties.ContainsKey("TargetName"))
{
MSceneObject sceneObject = this.SceneAccess.GetSceneObjectByName(instruction.Properties["TargetName"]);
if (sceneObject != null)
{
return(sceneObject.Transform);
}
}
return(null);
}
/// <summary>
/// Dynamically estimates the target transform
/// </summary>
/// <returns></returns>
private MTransform ComputeTargetTransform()
{
MTransform target = new MTransform()
{
ID = "target"
};
//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"]);
//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(instruction.Properties["TargetID"]);
}
return(target);
}
private void OnGUI()
{
if (GUI.Button(new Rect(100, 100, 100, 50), "Walk"))
{
MConstraint walkConstraint = new MConstraint(System.Guid.NewGuid().ToString())
{
GeometryConstraint = new MGeometryConstraint("")
{
ParentToConstraint = new MTransform("", new MVector3(1, 0, 3), new MQuaternion(0, 0, 0, 1))
}
};
MInstruction walkInstruction = new MInstruction("walkXY", "walk", "walk")
{
Properties = new Dictionary <string, string>()
{
{ "TargetID", walkConstraint.ID }
},
Constraints = new List <MConstraint>()
{
walkConstraint
}
};
this.GetComponent <MMIAvatar>().CoSimulator.AssignInstruction(walkInstruction, null);
}
}
private void OnGUI()
{
if (GUI.Button(new Rect(270, 100, 200, 25), "Reach (MConstraint)"))
{
MConstraint reachConstraint = new MConstraint(System.Guid.NewGuid().ToString())
{
GeometryConstraint = new MGeometryConstraint("")
{
ParentToConstraint = new MTransform("", this.ReachTarget.MSceneObject.Transform.Position, this.ReachTarget.MSceneObject.Transform.Rotation)
}
};
MInstruction reachInstruction = new MInstruction("reachXY", "reach", "Pose/Reach")
{
Properties = new Dictionary <string, string>()
{
{ "TargetID", reachConstraint.ID },
{ "Hand", this.Hand.ToString() }
},
Constraints = new List <MConstraint>()
{
reachConstraint
}
};
this.GetComponent <MMIAvatar>().CoSimulator.AssignInstruction(reachInstruction, null);
}
}
/// <summary>
/// Returns a joint constraint representing the hand posture
/// </summary>
/// <returns></returns>
public List <MConstraint> GetJointConstraints()
{
List <MConstraint> constraints = new List <MConstraint>();
foreach (MJoint joint in this.GetPose().Joints)
{
//Create a new constraint
MConstraint constraint = new MConstraint(Guid.NewGuid().ToString())
{
JointConstraint = new MJointConstraint(joint.Type)
{
GeometryConstraint = new MGeometryConstraint()
{
ParentObjectID = "",
ParentToConstraint = new MTransform()
{
ID = Guid.NewGuid().ToString(),
Position = joint.Position,
Rotation = joint.Rotation
}
}
}
};
constraints.Add(constraint);
}
return(constraints);
}
/// <summary>
/// Returns the endeffector constraint
/// </summary>
/// <param name="type"></param>
/// <returns></returns>
public virtual MJointConstraint GetEndeffectorConstraint(MJointType type)
{
MConstraint constraint = this.constraints.Find(s => s.JointConstraint != null && s.JointConstraint.JointType == type);
if (constraint == null || constraint.JointConstraint == null)
{
return(null);
}
return(constraint.JointConstraint);
}
/// <summary>
/// Returns the endeffector constraint
/// </summary>
/// <param name="type"></param>
/// <returns></returns>
public virtual MJointConstraint GetEndeffectorConstraint(string id)
{
MConstraint constraint = this.constraints.Find(s => s.JointConstraint != null && s.ID == id);
if (constraint == null || constraint.JointConstraint == null)
{
return(null);
}
return(constraint.JointConstraint);
}
/// <summary>
/// Sets a new endeffector constraint and replaces old ones if available
/// </summary>
/// <param name="newConstraint"></param>
/// <returns></returns>
public virtual void SetEndeffectorConstraint(MJointConstraint newConstraint, String id = null)
{
//Create a new id if null
if (id == null)
{
id = Guid.NewGuid().ToString();
}
this.RemoveEndeffectorConstraints(newConstraint.JointType);
//Create a new constraint
MConstraint constraint = new MConstraint(id)
{
JointConstraint = newConstraint
};
this.constraints.Add(constraint);
}
/// <summary>
/// Sets up the respective hand using the given parameters
/// </summary>
/// <param name="jointType"></param>
/// <param name="instruction"></param>
/// <param name="duration"></param>
/// <param name="angularVelocity"></param>
/// <param name="durationSet"></param>
/// <param name="release"></param>
private void SetupHand(MJointType jointType, MInstruction instruction, float duration, float angularVelocity, bool durationSet, bool release)
{
//Create a new hand container
HandContainer hand = new HandContainer()
{
Type = jointType,
Instruction = instruction,
Release = release,
Duration = duration,
AngularVelocity = angularVelocity,
HasDuration = durationSet,
Positioned = false
};
HandContainer old = this.ActiveHands.Find(s => s.Type == jointType);
if (old != null)
{
this.ActiveHands.Remove(old);
}
//Handle the hand pose (if defined)
if (instruction.Properties.ContainsKey("HandPose"))
{
//Get the constraint id
string constraintID = instruction.Properties["HandPose"];
//Get the corresponding hand constraint
MConstraint handConstraint = instruction.Constraints.Find(s => s.ID == constraintID);
//Check if hand constraint is defined and assign as final posture
if (handConstraint != null && handConstraint.PostureConstraint != null)
{
hand.FinalPosture = handConstraint.PostureConstraint;
}
}
//Add as active hand
this.ActiveHands.Add(hand);
}
/// <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));
}
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()
});
}
}
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));
}
