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)); }