HumanoidEntity CreateHumanoid(string name, Vector3 initialPosition)
{
Dictionary <string, VisualEntity> humanoidShapes = new Dictionary <string, VisualEntity>();
string prefix = name + "_";
foreach (HumanoidShapeDescriptor desc in ShapeDescriptors)
{
Shape newShape = null;
newShape = new BoxShape(new BoxShapeProperties(
desc.Name + " Shape", (float)desc.mass, new Pose(), new Vector3((float)desc.xSize, (float)desc.ySize, (float)desc.zSize)));
// newShape = new ConvexMeshShape(new ConvexMeshShapeProperties(desc.Name + "Shape", desc.mesh));
SingleShapeEntity shapeEntity = null;
/*
* SimplifiedConvexMeshEnvironmentEntity shapeEntity = null;
* if (desc.Name == _parentName)
* {
* shapeEntity = new HumanoidEntity(newShape, new Vector3(
* (float)desc.xPosition + initialPosition.X,
* (float)desc.yPosition + initialPosition.Y,
* (float)desc.zPosition + initialPosition.Z),desc.mesh);
* }
* else
* {
* shapeEntity = new SegmentEntity(newShape, new Vector3(
* (float)desc.xPosition + initialPosition.X,
* (float)desc.yPosition + initialPosition.Y,
* (float)desc.zPosition + initialPosition.Z),desc.mesh);
* }
*/
if (desc.Name == _parentName)
{
shapeEntity = new HumanoidEntity(newShape, new Vector3(
(float)desc.xPosition + initialPosition.X,
(float)desc.yPosition + initialPosition.Y,
(float)desc.zPosition + initialPosition.Z));
}
else
{
shapeEntity = new SegmentEntity(newShape, new Vector3(
(float)desc.xPosition + initialPosition.X,
(float)desc.yPosition + initialPosition.Y,
(float)desc.zPosition + initialPosition.Z));
}
shapeEntity.State.Name = prefix + desc.Name;
shapeEntity.State.Pose.Orientation = UIMath.EulerToQuaternion(new xna.Vector3((float)desc.xRotation, (float)desc.yRotation, (float)desc.zRotation));
if (!string.IsNullOrEmpty(desc.mesh))
{
shapeEntity.State.Assets.Mesh = desc.mesh;
}
shapeEntity.MeshScale = new Vector3(.001f, .001f, .001f);
shapeEntity.MeshRotation = new Vector3(desc.xRotation, desc.yRotation, desc.zRotation);
shapeEntity.Flags = VisualEntityProperties.DoCompletePhysicsShapeUpdate;
// shapeEntity.State.Flags |= EntitySimulationModifiers.Kinematic;
humanoidShapes.Add(shapeEntity.State.Name, shapeEntity);
}
// now set up the Parent/Child relationships
foreach (ParentChild rel in Relationships)
{
string Dof = rel.Dof;
JointAngularProperties angular = new JointAngularProperties();
if (Dof == "Twist")
{
angular.TwistMode = JointDOFMode.Free;
angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 1000, 0), 1000000);
// angular.TwistDrive.Mode = JointDriveMode.Velocity;
}
if (Dof == "Swing1")
{
angular.Swing1Mode = JointDOFMode.Free;
angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 1000, 0), 1000000);
//angular.SwingDrive.Mode = JointDriveMode.Velocity;
}
if (Dof == "Swing2")
{
angular.Swing2Mode = JointDOFMode.Free;
angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 1000, 0), 1000000);
// angular.SwingDrive.Mode = JointDriveMode.Velocity;
}
EntityJointConnector[] connectors = new EntityJointConnector[]
{
new EntityJointConnector(humanoidShapes[prefix + rel.Child], rel.ChildNormal, rel.ChildAxis, rel.ChildConnect),
new EntityJointConnector(humanoidShapes[prefix + rel.Parent], rel.ParentNormal, rel.ParentAxis, rel.ParentConnect)
};
SegmentEntity child = (SegmentEntity)humanoidShapes[prefix + rel.Child];
child.CustomJoint = new Joint();
child.CustomJoint.State = new JointProperties(angular, connectors);
child.CustomJoint.State.Name = rel.JointName;
child.Flags = VisualEntityProperties.DoCompletePhysicsShapeUpdate;
humanoidShapes[prefix + rel.Parent].InsertEntityGlobal(humanoidShapes[prefix + rel.Child]);
}
HumanoidEntity retValue = (HumanoidEntity)humanoidShapes[prefix + _parentName];
// retValue.State.MassDensity.CenterOfMass = new Pose(new Vector3(0.05f,1.05f,0), new Quaternion(0,0,0,1));
retValue.State.Flags = EntitySimulationModifiers.Kinematic;
retValue.State.Name = name;
return(retValue);
}
private void AddGripper(ref Vector3 position)
{
var upperShape = new BoxShape(new BoxShapeProperties(
"wrist", // name
100, // mass
new Pose(new Vector3(0, GripperHeight / 2f, 0), new Quaternion(0, 0, 0, 1)),
new Vector3(GripperWidth, GripperHeight, GripperDepth) // dimension
));
position = new Vector3(0, WristHeight-WristDepth/2f+0.009f, 0);
_gripperEntity = new SingleShapeSegmentEntity(upperShape, position);
_gripperEntity.State.Name = this.State.Name + " Gripper";
_gripperEntity.State.Flags |= EntitySimulationModifiers.DisableCollisions;
_gripperEntity.State.Assets.Mesh = @"RobotArm_Gripper.obj";
var jointAngularProperties = new JointAngularProperties
{
TwistMode = JointDOFMode.Limited,
TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000),
UpperTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(90), 0, new SpringProperties()),
LowerTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(-90), 0, new SpringProperties()),
};
var connectors = new[]
{
new EntityJointConnector(_gripperEntity, new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 0)),
new EntityJointConnector(_wristEntity, new Vector3(1, 0, 0), new Vector3(0, 1, 0), position)
};
_gripperEntity.CustomJoint = new Joint();
_gripperEntity.CustomJoint.State = new JointProperties(jointAngularProperties, connectors);
_gripperEntity.CustomJoint.State.Name = "WristRotate|-90|90|";
_gripperEntity.Parent = _wristEntity;
_wristEntity.InsertEntity(_gripperEntity);
_joints.Add(new JointDesc(_gripperEntity.CustomJoint.State.Name,
Conversions.RadiansToDegrees(jointAngularProperties.LowerTwistLimit.LimitThreshold),
Conversions.RadiansToDegrees(jointAngularProperties.UpperTwistLimit.LimitThreshold),
_gripperEntity, true));
//TODO: temporary
_joints.Add(new JointDesc("Grip", 0, 2f, _gripperEntity, false));
}
private void AddLowerArm(ref Vector3 position)
{
var lowerShape = new BoxShape(new BoxShapeProperties(
"lower arm", // name
100, // mass
new Pose(new Vector3(0, LowerHeight / 2f - LowerDepth/2f, 0), new Quaternion(0, 0, 0, 1)),
new Vector3(LowerWidth, LowerHeight, LowerDepth) // dimension
));
position = TopLowerJointOffset;
_lowerEntity = new SingleShapeSegmentEntity(lowerShape, position);
_lowerEntity.State.Name = this.State.Name + " Lower Arm";
_lowerEntity.State.Flags |= EntitySimulationModifiers.DisableCollisions;
_lowerEntity.State.Assets.Mesh = @"RobotArm_Lower.obj";
//_lowerEntity.MeshTranslation = new Vector3(-LowerWidth/2f, -0.012f, -LowerDepth/2f);
var jointAngularProperties = new JointAngularProperties
{
TwistMode = JointDOFMode.Limited,
TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, Conversions.DegreesToRadians(52)), 100000000),
UpperTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(54.1f), 0, new SpringProperties()),
LowerTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(-90.1f), 0, new SpringProperties()),
};
var connectors = new[]
{
new EntityJointConnector(_lowerEntity, new Vector3(0, 1, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 0)),
new EntityJointConnector(_topEntity, new Vector3(0, 1, 0), new Vector3(1, 0, 0), TopLowerJointOffset)
};
_lowerEntity.CustomJoint = new Joint();
_lowerEntity.CustomJoint.State = new JointProperties(jointAngularProperties, connectors);
_lowerEntity.CustomJoint.State.Name = "Shoulder|-90|54|";
_lowerEntity.Parent = _topEntity;
_topEntity.InsertEntity(_lowerEntity);
_joints.Add(new JointDesc(_lowerEntity.CustomJoint.State.Name,
Conversions.RadiansToDegrees(jointAngularProperties.LowerTwistLimit.LimitThreshold),
Conversions.RadiansToDegrees(jointAngularProperties.UpperTwistLimit.LimitThreshold),
_lowerEntity, false));
}
private void AddUpperArm(ref Vector3 position)
{
var upperShape = new BoxShape(new BoxShapeProperties(
"upper", // name
100, // mass
new Pose(new Vector3(0, UpperHeight/2f - UpperBottomJointOffset, 0), new Quaternion(0, 0, 0, 1)),
new Vector3(UpperWidth, UpperHeight, UpperDepth) // dimension
));
position = LowerUpperJointOffset;
_upperEntity = new SingleShapeSegmentEntity(upperShape, position);
_upperEntity.State.Name = this.State.Name + " Upper";
//_upperEntity.State.Flags |= EntitySimulationModifiers.DisableCollisions;
_upperEntity.State.Assets.Mesh = @"RobotArm_Upper.obj";
var jointAngularProperties = new JointAngularProperties
{
TwistMode = JointDOFMode.Limited,
TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000),
UpperTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(155), 0, new SpringProperties()),
LowerTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(-155), 0, new SpringProperties()),
};
var connectors = new[]
{
new EntityJointConnector(_upperEntity, new Vector3(0, 1, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 0)),
new EntityJointConnector(_lowerEntity, new Vector3(0, 1, 0), new Vector3(1, 0, 0), LowerUpperJointOffset)
};
_upperEntity.CustomJoint = new Joint();
_upperEntity.CustomJoint.State = new JointProperties(jointAngularProperties, connectors);
_upperEntity.CustomJoint.State.Name = "Elbow|-155|155|";
_upperEntity.Parent = _lowerEntity;
_lowerEntity.InsertEntity(_upperEntity);
_joints.Add(new JointDesc(_upperEntity.CustomJoint.State.Name,
Conversions.RadiansToDegrees(jointAngularProperties.LowerTwistLimit.LimitThreshold),
Conversions.RadiansToDegrees(jointAngularProperties.UpperTwistLimit.LimitThreshold),
_upperEntity, false));
}
private void AddTop(ref Vector3 position)
{
var topShape = new BoxShape(new BoxShapeProperties(
"top", // name
100, // mass
new Pose(new Vector3(0, TopHeight/2, 0), new Quaternion(0, 0, 0, 1)),
new Vector3(TopRadius*2, TopHeight, TopRadius*2)
));
position += new Vector3(0, BaseHeight + TopGap, 0);
_topEntity = new SingleShapeSegmentEntity(topShape, position);
_topEntity.State.Name = this.State.Name + " Top";
_topEntity.State.Assets.Mesh = @"RobotArm_Top.obj";
_topEntity.Parent = _baseEntity;
var topAngular = new JointAngularProperties
{
TwistMode = JointDOFMode.Limited,
TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000),
UpperTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(90.1f), 0, new SpringProperties()),
LowerTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(-90.1f), 0, new SpringProperties()),
};
var topConnector = new[]
{
new EntityJointConnector(_topEntity, new Vector3(1, 0, 0), new Vector3(0, -1, 0), new Vector3(0, 0, 0)),
new EntityJointConnector(_baseEntity, new Vector3(1, 0, 0), new Vector3(0, -1, 0), new Vector3(0, BaseHeight + TopGap, 0))
};
_topEntity.CustomJoint = new Joint();
_topEntity.CustomJoint.State = new JointProperties(topAngular, topConnector);
_topEntity.CustomJoint.State.Name = "BaseJoint|-90|90|";
// add as child of base entity
_baseEntity.InsertEntity(_topEntity);
_joints.Add(new JointDesc(_topEntity.CustomJoint.State.Name,
Conversions.RadiansToDegrees(topAngular.LowerTwistLimit.LimitThreshold),
Conversions.RadiansToDegrees(topAngular.UpperTwistLimit.LimitThreshold),
_topEntity, true));
}
