void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.VisualEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
CreateDefaultState();
}
void MoveBipedPose(Vector3 offset, simengine.VisualEntity entity)
{
entity.PhysicsEntity.SetPose(new Pose(offset + entity.State.Pose.Position, entity.State.Pose.Orientation));
foreach (simengine.VisualEntity child in entity.Children)
{
MoveBipedPose(offset, child);
}
}
public JointDesc(string name, simengine.VisualEntity jointEntity, PhysicsJoint joint)
{
Name = name;
JointEntity = jointEntity;
Swing1Angle = 0;
Swing2Angle = 0;
TwistAngle = 0;
X = Y = Z = 0;
Joint = joint;
}
void DeleteEntityInternal()
{
_entity = null;
// add a slider for each joint to the UI
WinFormsServicePort.FormInvoke(
delegate()
{
_simulatedBipedMoverUI.ClearNames();
_simulatedBipedMoverUI.ClearSliders();
Activate(Arbiter.Receive(false, TimeoutPort(5000), dateTime => SpawnIterator(RefreshListIterator)));
}
);
}
void SuspendBipedInternal(simengine.VisualEntity entity, bool suspend)
{
if (suspend)
{
entity.PhysicsEntity.IsKinematic = true;
entity.State.Flags |= Microsoft.Robotics.Simulation.Physics.EntitySimulationModifiers.Kinematic;
MoveBipedPose(new Vector3(0, 0.3f, 0), entity);
}
else
{
entity.State.Flags &= ~Microsoft.Robotics.Simulation.Physics.EntitySimulationModifiers.Kinematic;
MoveBipedPose(new Vector3(0, -0.28f, 0), entity);
Activate(Arbiter.Receive(false, TimeoutPort(200), SetDynamic));
}
}
void AddJoint(Joint joint, simengine.VisualEntity entity)
{
if (!(joint is PhysicsJoint))
{
return;
}
if (!_state.Joints.ContainsKey(joint.State.Name))
{
JointDesc description = new JointDesc(joint.State.Name, entity, joint as PhysicsJoint);
string[] DOFNames = joint.State.Name.Split(';');
int which = 0;
if (joint.State.Angular != null)
{
if (joint.State.Angular.TwistMode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Angular.TwistDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Twist);
}
if (joint.State.Angular.Swing1Mode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Angular.SwingDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Swing1);
}
if (joint.State.Angular.Swing2Mode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Angular.SlerpDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Swing2);
}
}
if (joint.State.Linear != null)
{
if (joint.State.Linear.XMotionMode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Linear.XDrive))
{
AddDOF(DOFNames, which++, description, DOFType.X);
}
if (joint.State.Linear.YMotionMode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Linear.YDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Y);
}
if (joint.State.Linear.ZMotionMode != JointDOFMode.Locked && IsNotUselessDrive(joint.State.Linear.ZDrive))
{
AddDOF(DOFNames, which++, description, DOFType.Z);
}
}
}
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.VisualEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
// traverse the entities associated with this entity and build a list of joints
_state.Joints.Clear();
Dictionary <string, simengine.VisualEntity> entities = new Dictionary <string, simengine.VisualEntity>();
FindJointsAndEntities(_entity, entities);
// add a slider for each joint to the UI
WinFormsServicePort.FormInvoke(
delegate()
{
_simulatedBipedMoverUI.AddEntityName(_entity.State.Name, _state.Joints == null || _state.Joints.Count == 0);
_simulatedBipedMoverUI.AddSliders(_state.Joints);
}
);
}
IEnumerator <ITask> OnInsertEntity(sime.InsertSimulationEntity insert)
{
_entity = insert.Body;
var query = new sime.VisualEntity();
query.State.Name = _state.CameraName;
yield return(Arbiter.Choice(
_simPort.Query(query),
success => _camera = success.Entity as Entities.PursuitCamera.PursuitCameraEntity,
failure => LogError("Unable to find camera", failure)
));
if (_camera == null)
{
_entity = null;
yield break;
}
SetCameraProperties();
_camera.Target = _entity;
}
void DeleteEntityNotificationHandler(simengine.DeleteSimulationEntity del)
{
_entity = null;
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (PioneerGPSEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
CreateDefaultState();
}
void OnDeleteEntity(sime.DeleteSimulationEntity delete)
{
_entity = null;
_camera = null;
}
private void FindJointsAndEntities(simengine.VisualEntity thisEntity, Dictionary <string, simengine.VisualEntity> visited)
{
if (thisEntity == null)
{
return;
}
if (visited.ContainsKey(thisEntity.State.Name))
{
return;
}
else
{
visited.Add(thisEntity.State.Name, thisEntity);
}
// process the children first
if (thisEntity is simengine.VisualEntity)
{
foreach (simengine.VisualEntity Child in ((simengine.VisualEntity)thisEntity).Children)
{
FindJointsAndEntities(Child, visited);
}
}
if (thisEntity.ParentJoint != null)
{
AddJoint(thisEntity.ParentJoint, thisEntity);
}
// search this entity for other entities or joints
Type thisEntityType = thisEntity.GetType();
System.Reflection.FieldInfo[] fields = thisEntityType.GetFields(
System.Reflection.BindingFlags.Instance |
System.Reflection.BindingFlags.Public |
System.Reflection.BindingFlags.NonPublic);
foreach (FieldInfo field in fields)
{
// search for joints
if ((field.FieldType == typeof(Joint)) || (field.FieldType.IsSubclassOf(typeof(Joint))))
{
Joint thisJoint = (Joint)field.GetValue(thisEntity);
if (thisJoint != null)
{
AddJoint(thisJoint, thisEntity);
}
}
else if (field.FieldType == typeof(List <Joint>))
{
List <Joint> jointList = (List <Joint>)field.GetValue(thisEntity);
if (jointList != null)
{
foreach (Joint thisJoint in jointList)
{
AddJoint(thisJoint, thisEntity);
}
}
}
else if (field.FieldType == typeof(Joint[]))
{
Joint[] jointArray = (Joint[])field.GetValue(thisEntity);
if (jointArray != null)
{
foreach (Joint thisJoint in jointArray)
{
AddJoint(thisJoint, thisEntity);
}
}
}
// search for entities
if ((field.FieldType == typeof(simtypes.Entity)) || (field.FieldType.IsSubclassOf(typeof(simtypes.Entity))))
{
FindJointsAndEntities((simengine.VisualEntity)field.GetValue(thisEntity), visited);
}
else if (field.FieldType.IsGenericType)
{
Type[] parms = field.FieldType.GetGenericArguments();
if (parms.Length == 1)
{
if ((parms[0] == typeof(simtypes.Entity)) || (parms[0].IsSubclassOf(typeof(simtypes.Entity))))
{
foreach (simengine.VisualEntity someEntity in (System.Collections.ICollection)field.GetValue(thisEntity))
{
FindJointsAndEntities(someEntity, visited);
}
}
}
}
}
}
void DeleteEntityNotificationHandler(simengine.DeleteSimulationEntity del)
{
_entity = null;
}
