Submit My Question - Recent Questions - Recent Answer
Msdn Tech >> Microsoft Robotics Studio >> Microsoft Robotics - Simulation

BIZARRE JOINT-ARM BEHAVIOR*

I have several limbs(boxes) connected together end on end, little similar to the Kuka arm but lying flat on the ground. The very last limb is perpendicular to the xz plane or ground in our simulation world. The very first limb (box) is a fixed (kinematic) arm. I have also created a holder or support for the perpendicular limb so that it stays within the bounds of the support. The thing is whenever I start the simulation, all the limbs including the perpendicular limb tend to fly towards the fixed limb. Which I suppose should not happen as it is against the laws of physics, leaving the support unaffected. Either the support is not holding the perpendicular limb or there is something wrong with the physics engine. So, if anyone knows a solution to a problem similar to this, do please help. Thank you.
[831 byte] By [Courageous] at [2008-1-8]
«« C# 2008 learning
»» Using WebClient to download file
# 1
Could you post the important snippets of your code, especially the definition of your joints? It's hard to tell what's going on without it. Maybe you haven't defined the Connection Points in the EntityConnectors.
bkloster at 2007-10-2 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 2

Here I attach my code for my RobotEntity, you'll have to create a simulation project and then add this as a file into the project, followed by creating the RobotEntity in the main file of the project. Thank you for your help.

And since MS doesn't allow more than 50000 characters of input in the forum per post I have posted the code in several chunks of snippet that has to be pasted together.

Code Snippet

#region Namespaces

using Microsoft.Ccr.Core;

using Microsoft.Dss.Core;

using Microsoft.Dss.Core.Attributes;

using Microsoft.Dss.ServiceModel.Dssp;

using Microsoft.Dss.ServiceModel.DsspServiceBase;

using System;

using System.Collections.Generic;

using System.Security.Permissions;

using System.ComponentModel;

using System.Xml;

using Microsoft.Xna.Framework.Graphics;

using Microsoft.Robotics.Simulation.Physics;

using Microsoft.Robotics.PhysicalModel;

using xna = Microsoft.Xna.Framework;

using xnagrfx = Microsoft.Xna.Framework.Graphics;

using xnaprof = Microsoft.Robotics.Simulation.MeshLoader;

using System.IO;

using System.Drawing;

#region Simulation namespaces

using simcommonproxy = Microsoft.Robotics.Simulation.Proxy;

using simcommon = Microsoft.Robotics.Simulation;

using Microsoft.Robotics.Simulation.Engine;

using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;

using Microsoft.Robotics.Simulation.Physics;

using arm = Microsoft.Robotics.Services.ArticulatedArm.Proxy;

using W3C.Soap;

using Microsoft.Robotics.PhysicalModel;

using physicalmodelproxy = Microsoft.Robotics.PhysicalModel.Proxy;

using System.ComponentModel;

using Microsoft.Robotics.Simulation;

#endregion

#endregion

namespace Robotics.Try

{

public class RobotEntity : VisualEntity

{

#region Variables and Constants

const float LIMB_MASS = 0.2f;

const float LIMB_LENGTH = 0.5f;

const float LIMB_WIDTH = 0.02f;

const float LIMB_THICKNESS = 0.01f;

const float SUPPORT_MASS = 0.01f;

const float SUPPORT_LENGTH = 0.2f;

const float SLIDER_MASS = 0.001f;

const float AXLE_MASS = 0.001f;

const float AXLE_RADIUS = LIMB_WIDTH / 2.0f;

const float AXLE_HEIGHT = 0.1f;

const float ROTOR_MASS = 0.001f;

const float ROTOR_LENGTH = 1.5f;

const float ROTOR_WIDTH = 0.03f;

const float ROTOR_THICKNESS = 0.001f;

const float BOX_MASS = 0.1f;

const float BOX_A_LENGTH = 2.0f * AXLE_RADIUS + 0.001f;

const float BOX_A_WIDTH = 0.01f;

const float BOX_B_LENGTH = 2.0f * AXLE_RADIUS + LIMB_THICKNESS + 0.1f + 0.002f;

const float BOX_B_WIDTH = 0.01f;

const float BOX_C_HEIGHT = AXLE_HEIGHT / 2.0f + 0.01f;

//const float BOX_C_HEIGHT = (LIMB_THICKNESS * 2.5f) + 0.001f;

Vector3 limbDimension = new Vector3(LIMB_LENGTH, LIMB_THICKNESS, LIMB_WIDTH);

Vector3 sliderDimension = new Vector3(LIMB_WIDTH, LIMB_THICKNESS, LIMB_WIDTH);

Vector3 supportDimension = new Vector3(SUPPORT_LENGTH, LIMB_THICKNESS, LIMB_WIDTH / 2.0f);

Vector3 radiusDimension = new Vector3(SUPPORT_LENGTH / 2.0f, LIMB_THICKNESS, LIMB_WIDTH);

Vector3 rotorDimension = new Vector3(ROTOR_WIDTH, ROTOR_THICKNESS, ROTOR_LENGTH);

Vector3 greyLimbPosition1 = new Vector3(-1.414f * LIMB_LENGTH - LIMB_LENGTH / 2.0f, LIMB_THICKNESS, 0);

Vector3 greyLimbPosition2 = new Vector3(1.414f * LIMB_LENGTH + LIMB_LENGTH / 2.0f, LIMB_THICKNESS, 0);

Vector3 darkLimbPosition1 = new Vector3(-1.0607f * LIMB_LENGTH, LIMB_THICKNESS, -0.3536f * LIMB_LENGTH);

Vector3 darkLimbPosition2 = new Vector3(-0.3536f * LIMB_LENGTH, LIMB_THICKNESS, -1.0607f * LIMB_LENGTH);

Vector3 darkLimbPosition3 = new Vector3(-0.3536f * LIMB_LENGTH, LIMB_THICKNESS, -0.3536f * LIMB_LENGTH);

Vector3 darkLimbPosition4 = new Vector3(0.3536f * LIMB_LENGTH, LIMB_THICKNESS, -1.0607f * LIMB_LENGTH);

Vector3 darkLimbPosition5 = new Vector3(0.3536f * LIMB_LENGTH, LIMB_THICKNESS, -0.3536f * LIMB_LENGTH);

Vector3 darkLimbPosition6 = new Vector3(1.0607f * LIMB_LENGTH, LIMB_THICKNESS, -0.3536f * LIMB_LENGTH);

Vector3 sliderPosition = new Vector3(1.414f * LIMB_LENGTH, LIMB_THICKNESS, 0);

Vector3 supportPosition1 = new Vector3(1.414f * LIMB_LENGTH, LIMB_THICKNESS, -3.0f * LIMB_WIDTH / 4.0f);

Vector3 supportPosition2 = new Vector3(1.414f * LIMB_LENGTH, LIMB_THICKNESS, 3.0f * LIMB_WIDTH / 4.0f);

Vector3 radiusPosition = new Vector3(2.414f * LIMB_LENGTH + SUPPORT_LENGTH / 4.0f, LIMB_THICKNESS, 0);

Vector3 axlePosition = new Vector3(2.414f * LIMB_LENGTH + 0.07f, AXLE_RADIUS, 0);

Vector3 rotorPosition = new Vector3(2.414f * LIMB_LENGTH + 0.07f, AXLE_RADIUS, 0);

Vector3 boxADimension = new Vector3(BOX_A_WIDTH, BOX_A_WIDTH, BOX_A_LENGTH);

Vector3 boxBDimension = new Vector3(BOX_B_LENGTH, BOX_B_WIDTH, BOX_B_WIDTH);

Vector3 boxCDimension = new Vector3(BOX_B_WIDTH, BOX_C_HEIGHT, BOX_B_WIDTH);

Vector3 boxA1Position = new Vector3(2.414f * LIMB_LENGTH + 0.07f - (BOX_A_LENGTH + BOX_A_WIDTH) / 2.0f, BOX_C_HEIGHT + BOX_A_WIDTH / 2.0f, 0);

Vector3 boxA2Position = new Vector3(2.414f * LIMB_LENGTH + 0.07f + (BOX_A_LENGTH + BOX_A_WIDTH) / 2.0f, BOX_C_HEIGHT + BOX_A_WIDTH / 2.0f, 0);

Vector3 boxB1Position = new Vector3(2.414f * LIMB_LENGTH + 0.07f + (BOX_B_LENGTH - BOX_A_LENGTH) / 2.0f - BOX_A_WIDTH, BOX_C_HEIGHT + BOX_A_WIDTH / 2.0f, -(BOX_A_LENGTH + BOX_A_WIDTH) / 2.0f);

Vector3 boxB2Position = new Vector3(2.414f * LIMB_LENGTH + 0.07f + (BOX_B_LENGTH - BOX_A_LENGTH) / 2.0f - BOX_A_WIDTH, BOX_C_HEIGHT + BOX_A_WIDTH / 2.0f, (BOX_A_LENGTH + BOX_A_WIDTH) / 2.0f);

Vector3 boxC1Position = new Vector3(2.414f * LIMB_LENGTH + 0.07f + BOX_B_LENGTH - (1.5f * BOX_B_WIDTH) - (BOX_A_LENGTH / 2.0f), BOX_C_HEIGHT / 2.0f, -(BOX_A_LENGTH + BOX_A_WIDTH) / 2.0f);

Vector3 boxC2Position = new Vector3(2.414f * LIMB_LENGTH + 0.07f + BOX_B_LENGTH - (1.5f * BOX_B_WIDTH) - (BOX_A_LENGTH / 2.0f), BOX_C_HEIGHT / 2.0f, (BOX_A_LENGTH + BOX_A_WIDTH) / 2.0f);

#endregion

#region Joints

#region Regular Joints

private PhysicsJoint _joint1 = null;

[DataMember]

public Joint Joint1

{

get { return _joint1; }

set { _joint1 = (PhysicsJoint)value; }

}

private PhysicsJoint _joint4 = null;

[DataMember]

public Joint Joint4

{

get { return _joint4; }

set { _joint4 = (PhysicsJoint)value; }

}

#endregion

#region Single Sided Joints for dark limbs

private PhysicsJoint _joint2 = null;

[DataMember]

public Joint Joint2

{

get { return _joint2; }

set { _joint2 = (PhysicsJoint)value; }

}

private PhysicsJoint _joint3 = null;

[DataMember]

public Joint Joint3

{

get { return _joint3; }

set { _joint3 = (PhysicsJoint)value; }

}

#endregion

#region Two Sided Joints

private PhysicsJoint _doubleJoint1 = null;

[DataMember]

public Joint DoubleJoint1

{

get { return _doubleJoint1; }

set { _doubleJoint1 = (PhysicsJoint)value; }

}

private PhysicsJoint _doubleJoint2 = null;

[DataMember]

public Joint DoubleJoint2

{

get { return _doubleJoint2; }

set { _doubleJoint2 = (PhysicsJoint)value; }

}

private PhysicsJoint _doubleJoint3 = null;

[DataMember]

public Joint DoubleJoint3

{

get { return _doubleJoint3; }

set { _doubleJoint3 = (PhysicsJoint)value; }

}

private PhysicsJoint _doubleJoint4 = null;

[DataMember]

public Joint DoubleJoint4

{

get { return _doubleJoint4; }

set { _doubleJoint4 = (PhysicsJoint)value; }

}

#endregion

#region Slider Joint

private PhysicsJoint _sliderJoint = null;

[DataMember]

public Joint SliderJoint

{

get { return _sliderJoint; }

set { _sliderJoint = (PhysicsJoint)value; }

}

#endregion

#region Rotating radius joint

private PhysicsJoint _radiusJoint = null;

[DataMember]

public Joint RadiusJoint

{

get { return _radiusJoint; }

set { _radiusJoint = (PhysicsJoint)value; }

}

#endregion

#region Rotating axle joint

private PhysicsJoint _axleJoint = null;

[DataMember]

public Joint AxleJoint

{

get { return _axleJoint; }

set { _axleJoint = (PhysicsJoint)value; }

}

#endregion

#region Rotor joint

private PhysicsJoint _rotorJoint = null;

[DataMember]

public Joint RotorJoint

{

get { return _rotorJoint; }

set { _rotorJoint = (PhysicsJoint)value; }

}

#endregion

#region Box Joints

private PhysicsJoint _boxJoint1 = null;

[DataMember]

public Joint BoxJoint1

{

get { return _boxJoint1; }

set { _boxJoint1 = (PhysicsJoint)value; }

}

private PhysicsJoint _boxJoint2 = null;

[DataMember]

public Joint BoxJoint2

{

get { return _boxJoint2; }

set { _boxJoint2 = (PhysicsJoint)value; }

}

private PhysicsJoint _boxJoint3 = null;

[DataMember]

public Joint BoxJoint3

{

get { return _boxJoint3; }

set { _boxJoint3 = (PhysicsJoint)value; }

}

private PhysicsJoint _boxJoint4 = null;

[DataMember]

public Joint BoxJoint4

{

get { return _boxJoint4; }

set { _boxJoint4 = (PhysicsJoint)value; }

}

private PhysicsJoint _boxJoint5 = null;

[DataMember]

public Joint BoxJoint5

{

get { return _boxJoint5; }

set { _boxJoint5 = (PhysicsJoint)value; }

}

private PhysicsJoint _boxJoint6 = null;

[DataMember]

public Joint BoxJoint6

{

get { return _boxJoint6; }

set { _boxJoint6 = (PhysicsJoint)value; }

}

#endregion

#endregion

#region Entities

#region Limbs

#region Grey Limbs

private SingleShapeEntity _greyLimb1;

[DataMember]

public SingleShapeEntity GreyLimb1

{

get { return _greyLimb1; }

set { _greyLimb1 = value; }

}

private SingleShapeEntity _greyLimb2;

[DataMember]

public SingleShapeEntity GreyLimb2

{

get { return _greyLimb2; }

set { _greyLimb2 = value; }

}

#endregion

#region Dark Limbs

private SingleShapeEntity _darkLimb1;

[DataMember]

public SingleShapeEntity DarkLimb1

{

get { return _darkLimb1; }

set { _darkLimb1 = value; }

}

private SingleShapeEntity _darkLimb2;

[DataMember]

public SingleShapeEntity DarkLimb2

{

get { return _darkLimb2; }

set { _darkLimb2 = value; }

}

private SingleShapeEntity _darkLimb3;

[DataMember]

public SingleShapeEntity DarkLimb3

{

get { return _darkLimb3; }

set { _darkLimb3 = value; }

}

private SingleShapeEntity _darkLimb4;

[DataMember]

public SingleShapeEntity DarkLimb4

{

get { return _darkLimb4; }

set { _darkLimb4 = value; }

}

private SingleShapeEntity _darkLimb5;

[DataMember]

public SingleShapeEntity DarkLimb5

{

get { return _darkLimb5; }

set { _darkLimb5 = value; }

}

private SingleShapeEntity _darkLimb6;

[DataMember]

public SingleShapeEntity DarkLimb6

{

get { return _darkLimb6; }

set { _darkLimb6 = value; }

}

#endregion

#endregion

#region Slider and Block support

private SingleShapeEntity _slider;

[DataMember]

public SingleShapeEntity Slider

{

get { return _slider; }

set { _slider = value; }

}

private SingleShapeEntity _support1;

[DataMember]

public SingleShapeEntity Support1

{

get { return _support1; }

set { _support1 = value; }

}

private SingleShapeEntity _support2;

[DataMember]

public SingleShapeEntity Support2

{

get { return _support2; }

set { _support2 = value; }

}

#endregion

#region Radius

private SingleShapeEntity _radius;

[DataMember]

public SingleShapeEntity Radius

{

get { return _radius; }

set { _radius = value; }

}

#endregion

#region Axle

private SingleShapeEntity _axle;

[DataMember]

public SingleShapeEntity Axle

{

get { return _axle; }

set { _axle = value; }

}

#endregion

#region Rotor

private SingleShapeEntity _rotor;

[DataMember]

public SingleShapeEntity Rotor

{

get { return _rotor; }

set { _rotor = value; }

}

#endregion

#region Rotor, Axle Holder

private SingleShapeEntity _boxA1;

[DataMember]

public SingleShapeEntity BoxA1

{

get { return _boxA1; }

set { _boxA1 = value; }

}

private SingleShapeEntity _boxA2;

[DataMember]

public SingleShapeEntity BoxA2

{

get { return _boxA2; }

set { _boxA2 = value; }

}

private SingleShapeEntity _boxB1;

[DataMember]

public SingleShapeEntity BoxB1

{

get { return _boxB1; }

set { _boxB1 = value; }

}

private SingleShapeEntity _boxB2;

[DataMember]

public SingleShapeEntity BoxB2

{

get { return _boxB2; }

set { _boxB2 = value; }

}

private SingleShapeEntity _boxC1;

[DataMember]

public SingleShapeEntity BoxC1

{

get { return _boxC1; }

set { _boxC1 = value; }

}

private SingleShapeEntity _boxC2;

[DataMember]

public SingleShapeEntity BoxC2

{

get { return _boxC2; }

set { _boxC2 = value; }

}

#endregion

#endregion

Courageous at 2007-10-2 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 3

Code Snippet

#region Constructors

// Default Constructor

public RobotEntity()

{

}

public RobotEntity(Vector3 initialPosition)

{

base.State.Pose.Position = initialPosition;

}

#endregion

#region Joint creation

#region Joint between grey limb and dark limb

private void CreateJointBetweenGreyAndDarkLimb1()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_joint1 = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_joint1.State.Name = "JointBetweenFirstGreyAndDarkLimbs";

}

private void CreateJointBetweenGreyAndDarkLimb2()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_joint4 = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_joint4.State.Name = "JointBetweenLastGreyAndDarkLimbs";

}

#endregion

#region Double Joint between dark limbs

private void CreateDoubleJoint1()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_doubleJoint1 = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_doubleJoint1.State.Name = "DoubleJoint1";

}

private void CreateDoubleJoint2()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_doubleJoint2 = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_doubleJoint2.State.Name = "DoubleJoint2";

}

private void CreateDoubleJoint3()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_doubleJoint3 = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_doubleJoint3.State.Name = "DoubleJoint3";

}

private void CreateDoubleJoint4()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_doubleJoint4 = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_doubleJoint4.State.Name = "DoubleJoint4";

}

#endregion

#region Single Joint Between dark limbs

private void CreateSingleJoint1()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_joint2 = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_joint2.State.Name = "SingleJointBetweenDarkLimbs1";

}

private void CreateSingleJoint2()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_joint3 = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_joint3.State.Name = "SingleJointBetweenDarkLimbs2";

}

#endregion

#region Joint between Slider, grey limb 2 and dark limb 6

private void CreateSliderJoint()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_sliderJoint = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_sliderJoint.State.Name = "SliderJoint";

}

#endregion

#region Radius Joint

private void CreateRadiusJoint()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_radiusJoint = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_radiusJoint.State.Name = "RadiusJoint";

}

#endregion

#region Axle Joint

private void CreateAxleJoint()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_axleJoint = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_axleJoint.State.Name = "AxleJoint";

}

#endregion

#region Rotor Joint

private void CreateRotorJoint()

{

JointAngularProperties commonAngular = new JointAngularProperties();

commonAngular.TwistMode = JointDOFMode.Free;

_rotorJoint = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));

_rotorJoint.State.Name = "RotorJoint";

}

#region Rotor, Axle support Joint

private void CreateBoxJoint1()

{

_boxJoint1 = PhysicsJoint.Create(new JointProperties());

_boxJoint1.State.Name = "BoxJoint1";

}

private void CreateBoxJoint2()

{

_boxJoint2 = PhysicsJoint.Create(new JointProperties());

_boxJoint2.State.Name = "BoxJoint2";

}

private void CreateBoxJoint3()

{

_boxJoint3 = PhysicsJoint.Create(new JointProperties());

_boxJoint3.State.Name = "BoxJoint3";

}

private void CreateBoxJoint4()

{

_boxJoint4 = PhysicsJoint.Create(new JointProperties());

_boxJoint4.State.Name = "BoxJoint4";

}

private void CreateBoxJoint5()

{

_boxJoint5 = PhysicsJoint.Create(new JointProperties());

_boxJoint5.State.Name = "BoxJoint5";

}

private void CreateBoxJoint6()

{

_boxJoint6 = PhysicsJoint.Create(new JointProperties());

_boxJoint6.State.Name = "BoxJoint6";

}

#endregion

#endregion

#endregion

#region Create Physical Entities

#region Create Limbs

#region Create Grey Limbs

private void CreateGreyLimb1()

{

BoxShapeProperties property = new BoxShapeProperties(

"GreyLimb1",

LIMB_MASS,

new Pose(),

limbDimension);

Shape shape = new BoxShape(property);

_greyLimb1 = new SingleShapeEntity(shape, base.State.Pose.Position);

_greyLimb1.State.MassDensity.Mass = LIMB_MASS;

_greyLimb1.State.Name = "GreyLimb1";

_greyLimb1.State.Pose.Position = greyLimbPosition1;

}

private void CreateGreyLimb2()

{

BoxShapeProperties property = new BoxShapeProperties(

"GreyLimb2",

LIMB_MASS,

new Pose(),

limbDimension);

Shape shape = new BoxShape(property);

_greyLimb2 = new SingleShapeEntity(shape, base.State.Pose.Position);

_greyLimb2.State.MassDensity.Mass = LIMB_MASS;

_greyLimb2.State.Name = "GreyLimb2";

_greyLimb2.State.Pose.Position = greyLimbPosition2;

}

#endregion

#region Create Dark Limbs

private void CreateDarkLimb1()

{

BoxShapeProperties property = new BoxShapeProperties(

"DarkLimb1",

LIMB_MASS,

new Pose(),//,darkLimbQuaternion1),

limbDimension);

Shape shape = new BoxShape(property);

_darkLimb1 = new SingleShapeEntity(shape, base.State.Pose.Position);

_darkLimb1.State.MassDensity.Mass = LIMB_MASS;

_darkLimb1.State.Name = "DarkLimb1";

_darkLimb1.State.Pose.Position = darkLimbPosition1;

}

private void CreateDarkLimb2()

{

BoxShapeProperties property = new BoxShapeProperties(

"DarkLimb2",

LIMB_MASS,

new Pose(),//,darkLimbQuaternion1),

limbDimension);

Shape shape = new BoxShape(property);

_darkLimb2 = new SingleShapeEntity(shape, base.State.Pose.Position);

_darkLimb2.State.MassDensity.Mass = LIMB_MASS;

_darkLimb2.State.Name = "DarkLimb2";

_darkLimb2.State.Pose.Position = darkLimbPosition2;

}

private void CreateDarkLimb3()

{

BoxShapeProperties property = new BoxShapeProperties(

"DarkLimb2",

LIMB_MASS,

new Pose(),//,darkLimbQuaternion1),

limbDimension);

Shape shape = new BoxShape(property);

_darkLimb3 = new SingleShapeEntity(shape, base.State.Pose.Position);

_darkLimb3.State.MassDensity.Mass = LIMB_MASS;

_darkLimb3.State.Name = "DarkLimb3";

_darkLimb3.State.Pose.Position = darkLimbPosition3;

}

private void CreateDarkLimb4()

{

BoxShapeProperties property = new BoxShapeProperties(

"DarkLimb4",

LIMB_MASS,

new Pose(),//,darkLimbQuaternion1),

limbDimension);

Shape shape = new BoxShape(property);

_darkLimb4 = new SingleShapeEntity(shape, base.State.Pose.Position);

_darkLimb4.State.MassDensity.Mass = LIMB_MASS;

_darkLimb4.State.Name = "DarkLimb4";

_darkLimb4.State.Pose.Position = darkLimbPosition4;

}

private void CreateDarkLimb5()

{

BoxShapeProperties property = new BoxShapeProperties(

"DarkLimb5",

LIMB_MASS,

new Pose(),//,darkLimbQuaternion1),

limbDimension);

Shape shape = new BoxShape(property);

_darkLimb5 = new SingleShapeEntity(shape, base.State.Pose.Position);

_darkLimb5.State.MassDensity.Mass = LIMB_MASS;

_darkLimb5.State.Name = "DarkLimb5";

_darkLimb5.State.Pose.Position = darkLimbPosition5;

}

private void CreateDarkLimb6()

{

BoxShapeProperties property = new BoxShapeProperties(

"DarkLimb6",

LIMB_MASS,

new Pose(),//,darkLimbQuaternion1),

limbDimension);

Shape shape = new BoxShape(property);

_darkLimb6 = new SingleShapeEntity(shape, base.State.Pose.Position);

_darkLimb6.State.MassDensity.Mass = LIMB_MASS;

_darkLimb6.State.Name = "DarkLimb6";

_darkLimb6.State.Pose.Position = darkLimbPosition6;

}

#endregion

#endregion

#region Create Slider and Support Blocks

private void CreateSlider()

{

BoxShapeProperties property = new BoxShapeProperties(

"Slider",

SLIDER_MASS,

new Pose(),

sliderDimension

);

Shape shape = new BoxShape(property);

_slider = new SingleShapeEntity(shape, base.State.Pose.Position);

_slider.State.MassDensity.Mass = SLIDER_MASS;

_slider.State.Name = "Slider";

_slider.State.Pose.Position = sliderPosition;

}

private void CreateSupport1()

{

BoxShapeProperties property = new BoxShapeProperties(

"Support1",

SUPPORT_MASS,

new Pose(),

supportDimension

);

Shape shape = new BoxShape(property);

_support1 = new SingleShapeEntity(shape, base.State.Pose.Position);

_support1.State.Name = "Support1";

_support1.State.Pose.Position = supportPosition1;

}

private void CreateSupport2()

{

BoxShapeProperties property = new BoxShapeProperties(

"Support2",

SUPPORT_MASS,

new Pose(),

supportDimension

);

Shape shape = new BoxShape(property);

_support2 = new SingleShapeEntity(shape, base.State.Pose.Position);

_support2.State.Name = "Support2";

_support2.State.Pose.Position = supportPosition2;

}

#endregion

#region Create Radius

private void CreateRadius()

{

BoxShapeProperties property = new BoxShapeProperties(

"Radius",

0.1f * LIMB_MASS,

new Pose(),

radiusDimension

);

Shape shape = new BoxShape(property);

_radius = new SingleShapeEntity(shape, base.State.Pose.Position);

_radius.State.Name = "Radius";

_radius.State.Pose.Position = radiusPosition;

}

#endregion

#region Create Axle

private void CreateAxle()

{

CapsuleShapeProperties property = new CapsuleShapeProperties(

"Axle",

AXLE_MASS,

new Pose(),

AXLE_RADIUS,

AXLE_HEIGHT

);

Shape shape = new CapsuleShape(property);

_axle = new SingleShapeEntity(shape, base.State.Pose.Position);

_axle.State.Name = "Axle";

_axle.State.Pose.Position = axlePosition;

}

#endregion

#region Create Rotor

private void CreateRotor()

{

BoxShapeProperties property = new BoxShapeProperties(

"Rotor",

ROTOR_MASS,

new Pose(),

rotorDimension

);

Shape shape = new BoxShape(property);

_rotor = new SingleShapeEntity(shape, base.State.Pose.Position);

_rotor.State.Name = "Rotor";

_rotor.State.Pose.Position = rotorPosition;

}

#endregion

#region Create Boxes

private void CreateBoxA1()

{

BoxShapeProperties property = new BoxShapeProperties(

"BoxA1",

BOX_MASS,

new Pose(),

boxADimension

);

Shape shape = new BoxShape(property);

_boxA1 = new SingleShapeEntity(shape, base.State.Pose.Position);

_boxA1.State.Name = "BoxA1";

_boxA1.State.Pose.Position = boxA1Position;

}

private void CreateBoxA2()

{

BoxShapeProperties property = new BoxShapeProperties(

"BoxA2",

BOX_MASS,

new Pose(),

boxADimension

);

Shape shape = new BoxShape(property);

_boxA2 = new SingleShapeEntity(shape, base.State.Pose.Position);

_boxA2.State.Name = "BoxA2";

_boxA2.State.Pose.Position = boxA2Position;

}

private void CreateBoxB1()

{

BoxShapeProperties property = new BoxShapeProperties(

"BoxB1",

BOX_MASS,

new Pose(),

boxBDimension

);

Shape shape = new BoxShape(property);

_boxB1 = new SingleShapeEntity(shape, base.State.Pose.Position);

_boxB1.State.Name = "BoxB1";

_boxB1.State.Pose.Position = boxB1Position;

}

private void CreateBoxB2()

{

BoxShapeProperties property = new BoxShapeProperties(

"BoxB2",

BOX_MASS,

new Pose(),

boxBDimension

);

Shape shape = new BoxShape(property);

_boxB2 = new SingleShapeEntity(shape, base.State.Pose.Position);

_boxB2.State.Name = "BoxB2";

_boxB2.State.Pose.Position = boxB2Position;

}

private void CreateBoxC1()

{

BoxShapeProperties property = new BoxShapeProperties(

"BoxC1",

BOX_MASS,

new Pose(),

boxCDimension

);

Shape shape = new BoxShape(property);

_boxC1 = new SingleShapeEntity(shape, base.State.Pose.Position);

_boxC1.State.Name = "BoxC1";

_boxC1.State.Pose.Position = boxC1Position;

}

private void CreateBoxC2()

{

BoxShapeProperties property = new BoxShapeProperties(

"BoxC2",

BOX_MASS,

new Pose(),

boxCDimension

);

Shape shape = new BoxShape(property);

_boxC2 = new SingleShapeEntity(shape, base.State.Pose.Position);

_boxC2.State.Name = "BoxC2";

_boxC2.State.Pose.Position = boxC2Position;

}

#endregion

#endregion

Courageous at 2007-10-2 > top of Msdn Tech,Microsoft Robotics Studio,Microsoft Robotics - Simulation...
# 4

Code Snippet

#region Initialize

public override void Initialize(GraphicsDevice device, PhysicsEngine physicsEngine)

{

try

{

InitError = string.Empty;

#region Initialize, Create, Connect and Insert the Limbs and the Joints

#region Initialize and create all the Joints

if (_joint1 == null)

{

CreateJointBetweenGreyAndDarkLimb1();

}

else

{

_joint1 = PhysicsJoint.Create(_joint1.State);

}

if (_joint2 == null)

{

CreateSingleJoint1();

}

else

{

_joint2 = PhysicsJoint.Create(_joint2.State);

}

if (_joint3 == null)

{

CreateSingleJoint2();

}

else

{

_joint3 = PhysicsJoint.Create(_joint3.State);

}

if (_joint4 == null)

{

CreateJointBetweenGreyAndDarkLimb2();

}

else

{

_joint4 = PhysicsJoint.Create(_joint4.State);

}

if (_doubleJoint1 == null)

{

CreateDoubleJoint1();

}

else

{

_doubleJoint1 = PhysicsJoint.Create(_doubleJoint1.State);

}

if (_doubleJoint2 == null)

{

CreateDoubleJoint2();

}

else

{

_doubleJoint2 = PhysicsJoint.Create(_doubleJoint2.State);

}

if (_doubleJoint3 == null)

{

CreateDoubleJoint3();

}

else

{

_doubleJoint3 = PhysicsJoint.Create(_doubleJoint3.State);

}

if (_doubleJoint4 == null)

{

CreateDoubleJoint4();

}

else

{

_doubleJoint4 = PhysicsJoint.Create(_doubleJoint4.State);

}

if (_sliderJoint == null)

{

CreateSliderJoint();

}

else

{

_sliderJoint = PhysicsJoint.Create(_sliderJoint.State);

}

if (_radiusJoint == null)

{

CreateRadiusJoint();

}

else

{

_radiusJoint = PhysicsJoint.Create(_radiusJoint.State);

}

if (_axleJoint == null)

{

CreateAxleJoint();

}

else

{

_axleJoint = PhysicsJoint.Create(_axleJoint.State);

}

if (_rotorJoint == null)

{

CreateRotorJoint();

}

else

{

_rotorJoint = PhysicsJoint.Create(_rotorJoint.State);

}

if (_boxJoint1 == null)

{

CreateBoxJoint1();

}

else

{

_boxJoint1 = PhysicsJoint.Create(_boxJoint1.State);

}

if (_boxJoint2 == null)

{

CreateBoxJoint2();

}

else

{

_boxJoint2 = PhysicsJoint.Create(_boxJoint2.State);

}

if (_boxJoint3 == null)

{

CreateBoxJoint3();

}

else

{

_boxJoint3 = PhysicsJoint.Create(_boxJoint3.State);

}

if (_boxJoint4 == null)

{

CreateBoxJoint4();

}

else

{

_boxJoint4 = PhysicsJoint.Create(_boxJoint4.State);

}

if (_boxJoint5 == null)

{

CreateBoxJoint5();

}

else

{

_boxJoint5 = PhysicsJoint.Create(_boxJoint5.State);

}

if (_boxJoint6 == null)

{

CreateBoxJoint6();

}

else

{

_boxJoint6 = PhysicsJoint.Create(_boxJoint6.State);

}

#endregion

#region Create the Limbs

if (_greyLimb1 == null) CreateGreyLimb1();

if (_greyLimb2 == null) CreateGreyLimb2();

if (_darkLimb1 == null) CreateDarkLimb1();

if (_darkLimb2 == null) CreateDarkLimb2();

if (_darkLimb3 == null) CreateDarkLimb3();

if (_darkLimb4 == null) CreateDarkLimb4();

if (_darkLimb5 == null) CreateDarkLimb5();

if (_darkLimb6 == null) CreateDarkLimb6();

if (_slider == null) CreateSlider();

if (_support1 == null) CreateSupport1();

if (_support2 == null) CreateSupport2();

if (_radius == null) CreateRadius();

if (_axle == null) CreateAxle();

if (_rotor == null) CreateRotor();

#region Create the boxes

if (_boxA1 == null) CreateBoxA1();

if (_boxA2 == null) CreateBoxA2();

if (_boxB1 == null) CreateBoxB1();

if (_boxB2 == null) CreateBoxB2();

if (_boxC1 == null) CreateBoxC1();

if (_boxC2 == null) CreateBoxC2();

#endregion

#endregion

#region Initialize the base class

base.Initialize(device, physicsEngine);

#endregion

#region Initialize the Limbs

_greyLimb1.Initialize(device, physicsEngine);

_greyLimb2.Initialize(device, physicsEngine);

_darkLimb1.Initialize(device, physicsEngine);

_darkLimb2.Initialize(device, physicsEngine);

_darkLimb3.Initialize(device, physicsEngine);

_darkLimb4.Initialize(device, physicsEngine);

_darkLimb5.Initialize(device, physicsEngine);

_darkLimb6.Initialize(device, physicsEngine);

_slider.Initialize(device, physicsEngine);

_support1.Initialize(device, physicsEngine);

_support2.Initialize(device, physicsEngine);

_radius.Initialize(device, physicsEngine);

_axle.Initialize(device, physicsEngine);

_rotor.Initialize(device, physicsEngine);

#region Initialize the boxes

_boxA1.Initialize(device, physicsEngine);

_boxA2.Initialize(device, physicsEngine);

_boxB1.Initialize(device, physicsEngine);

_boxB2.Initialize(device, physicsEngine);

_boxC1.Initialize(device, physicsEngine);

_boxC2.Initialize(device, physicsEngine);

#endregion

#endregion

#region Connect the Limbs

#region Connect Limbs with Single Joints

#region Grey limb and Dark Limb left hand side

_joint1.State.Connectors[0] = new EntityJointConnector(

_greyLimb1,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_joint1.State.Connectors[1] = new EntityJointConnector(

_darkLimb1,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-LIMB_LENGTH / 2.0f + 0.01f, -LIMB_THICKNESS / 2.0f, 0.0f)

);

#endregion

#region Dark Limb 2 and Dark Limb 4

_joint2.State.Connectors[0] = new EntityJointConnector(

_darkLimb2,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_joint2.State.Connectors[1] = new EntityJointConnector(

_darkLimb4,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-LIMB_LENGTH / 2.0f + 0.01f, -LIMB_THICKNESS / 2.0f, 0.0f)

);

#endregion

#region Dark Limb 3 and Dark Limb 5

_joint3.State.Connectors[0] = new EntityJointConnector(

_darkLimb3,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_joint3.State.Connectors[1] = new EntityJointConnector(

_darkLimb5,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-LIMB_LENGTH / 2.0f + 0.01f, -LIMB_THICKNESS / 2.0f, 0.0f)

);

#endregion

#region Dark Limb 6 and Grey Limb right hand side

_joint4.State.Connectors[0] = new EntityJointConnector(

_darkLimb6,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f, -LIMB_THICKNESS / 2.0f, 0.0f)

);

_joint4.State.Connectors[1] = new EntityJointConnector(

_greyLimb2,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-LIMB_LENGTH / 2.0f + 0.01f, LIMB_THICKNESS / 2.0f, 0.0f)

);

#endregion

#region Grey Limb and Slider

_sliderJoint.State.Connectors[0] = new EntityJointConnector(

_greyLimb2,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-LIMB_LENGTH / 2.0f, -LIMB_THICKNESS / 2.0f, 0.0f)

);

_sliderJoint.State.Connectors[1] = new EntityJointConnector(

_slider,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(0.0f, LIMB_THICKNESS / 2.0f, 0.0f)

);

#endregion

#region Grey Limb and the Radius

_radiusJoint.State.Connectors[0] = new EntityJointConnector(

_greyLimb2,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_radiusJoint.State.Connectors[1] = new EntityJointConnector(

_radius,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-SUPPORT_LENGTH / 4.0f + 0.02f, -LIMB_THICKNESS / 2.0f, 0.0f)

);

#endregion

#region Radius and the axle

_axleJoint.State.Connectors[0] = new EntityJointConnector(

_radius,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(SUPPORT_LENGTH / 4.0f - 0.02f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_axleJoint.State.Connectors[1] = new EntityJointConnector(

_axle,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-AXLE_RADIUS, -AXLE_HEIGHT / 2.0f + 0.005f, 0.0f)

);

#endregion

#region Axle and the Rotor

_rotorJoint.State.Connectors[0] = new EntityJointConnector(

_axle,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(0, AXLE_HEIGHT / 2.0f - 0.001f, 0)

);

_rotorJoint.State.Connectors[1] = new EntityJointConnector(

_rotor,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(0, ROTOR_THICKNESS / 2.0f, 0)

);

#endregion

#endregion

#region Connect Limbs with Double Joints

_doubleJoint1.State.Connectors[0] = new EntityJointConnector(

_darkLimb1,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_doubleJoint1.State.Connectors[1] = new EntityJointConnector(

_darkLimb2,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-LIMB_LENGTH / 2.0f + 0.01f, -LIMB_THICKNESS / 2.0f, 0.0f)

);

_doubleJoint2.State.Connectors[0] = new EntityJointConnector(

_darkLimb1,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_doubleJoint2.State.Connectors[1] = new EntityJointConnector(

_darkLimb3,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-LIMB_LENGTH / 2.0f + 0.01f, -LIMB_THICKNESS, 0.0f)

);

_doubleJoint3.State.Connectors[0] = new EntityJointConnector(

_darkLimb4,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_doubleJoint3.State.Connectors[1] = new EntityJointConnector(

_darkLimb6,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(-LIMB_LENGTH / 2.0f + 0.01f, -LIMB_THICKNESS / 2.0f, 0.0f)

);

_doubleJoint4.State.Connectors[0] = new EntityJointConnector(

_darkLimb4,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f - 0.005f, LIMB_THICKNESS / 2.0f, 0.0f)

);

_doubleJoint4.State.Connectors[1] = new EntityJointConnector(

_darkLimb5,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(LIMB_LENGTH / 2.0f - 0.005f, LIMB_THICKNESS, 0.0f)

);

#endregion

#region Connect the Boxes

_boxJoint1.State.Connectors[0] = new EntityJointConnector(

_boxA1,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(0, 0, -BOX_A_LENGTH / 2.0f)

);

_boxJoint1.State.Connectors[1] = new EntityJointConnector(

_boxB1,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3( (BOX_A_WIDTH - BOX_B_LENGTH) / 2.0f, 0, 0)

);

_boxJoint2.State.Connectors[0] = new EntityJointConnector(

_boxA1,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(0, 0, BOX_A_LENGTH / 2.0f)

);

_boxJoint2.State.Connectors[1] = new EntityJointConnector(

_boxB2,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3( (BOX_A_WIDTH - BOX_B_LENGTH) / 2.0f, 0, 0)

);

_boxJoint3.State.Connectors[0] = new EntityJointConnector(

_boxB1,

new Vector3(0, 0, 1),

new Vector3(0, 1, 0),

new Vector3(BOX_A_LENGTH + 1.5f * BOX_A_WIDTH - BOX_B_LENGTH / 2.0f, 0, 0)

);

_boxJoint3.State.Connectors[1] = new EntityJointConnector(

_boxA2,

new