Alan Oursland

I modified the Kuka arm to create a simple free swinging pendulum. The entity consists of two SingleShapeEntities connected by a single joint. One of the shape entities is set to be Kinematic so that it cannot be moved by physics.

Some things are not working and I am not sure why.

How to test the pendulum:

  1. Build SimplePendulum into an assembly and make sure the assembly is under bin/services.
  2. Start the simulator using the shortcut installed by MSRS.
  3. Press F3 to pause the physics engine.
  4. Press F5 to open the editor.
  5. Select 'detailed sphere' and press Ctrl-X to remove it. Repeat for 'box' and 'simple ground'.
  6. Select Entity-New. Select your assembly. Select the SimplePendulum class. Enter 'p' for the name. Press OK.
  7. Leave the position as 0,0,0. Press OK. The pendulum is added but the SingleShapeEntity components are not added.
  8. Press F2 three times to turn on combined rendering. Drag with the mouse to pan down to view the pendulum.
  9. Press F3 to run the physics engine. The shapes move to the positions they should be in.
  10. Select Physics-Settings. Select 'Enable rigid body for default camera' and press OK.
  11. Maneuver the camera sphere to collide with the pendulum to set it into motion.

Things that do not work:

  • Normal rendering: If I add a SingleShapeEntity it renders, but not as a child of a VisualEntity.
    - I tried copying MultiShapeEntity by adding the child shapes to the parent state physics entity with no success.
    - The Kuka arm calls each link in the Render override. This does not work either (I suspect because I am not using a mesh).
  • Relative position: If I change the positions of the parent, the child objects do not reposition.
  • Serialization: If I save a scene and reload it, the child entities are not present.
  • Extra object: An extra object is added by the pendulum. It looks like a small sphere and I suspect it is the parent object. How do I get rid of this
  • Odd initial position: The child objects start out in weird places and do not go to the correct postion until I start the physics engine. Why is this Also, if I try to change the child positions in the editor the scale is off by a factor of 10.
  • The JointConnectors parameters do not make sense to me. I just played with them until I got the result I wanted. Why are the joint axises in a different orientation than the rest of the axises

Code Snippet

[DataContract]
[CLSCompliant(true)]
public class SimplePendulum : VisualEntity {
private PhysicsJoint _joint = null;
private SingleShapeEntity _base;
private SingleShapeEntity _pend;
[DataMember]
public Joint Joint {
get { return _joint; }
set { _joint = (PhysicsJoint)value; }
}
[DataMember]
public SingleShapeEntity Base {
get { return _base; }
set { _base = value; }
}
[DataMember]
public SingleShapeEntity Pendulum {
get { return _pend; }
set { _pend = value; }
}
public SimplePendulum() {
}
public SimplePendulum(Vector3 position) {
State.Pose.Position = position;
}
private void CreateJoint() {
JointAngularProperties commonAngular = new JointAngularProperties();
commonAngular.Swing2Mode = JointDOFMode.Free;
_joint = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));
_joint.State.Name = "Joint";
}
private void CreateBase() {
const float r = 0.05f;
const float l = 0.50f;
const float mass = 10.0f;
BoxShapeProperties prop = new BoxShapeProperties(
"base",
mass,
new Pose(new Vector3(0f, 0f, l/2)), // uses the parent pose
new Vector3(r, r, l));
Shape shape = new BoxShape(prop);
_base = new SingleShapeEntity(shape, prop.LocalPose.Position);
}
private void CreatePendulum() {
const float r = 0.05f;
const float l = 0.50f;
const float mass = 10.0f;
BoxShapeProperties prop = new BoxShapeProperties(
"child",
mass,
new Pose(new Vector3(0f, -l/2+r/2, -r/2)), // this will be determined by the joint
//new Pose(new Vector3(0f, -l/2 + r/2, +r/2 + 0.001f)),
new Vector3(r, l, r));
Shape shape = new BoxShape(prop);
_pend = new SingleShapeEntity(shape, prop.LocalPose.Position);
}
public override void Initialize(xnagfx.GraphicsDevice device, PhysicsEngine physicsEngine) {
try {
InitError = string.Empty;
// programmatically build articulated arm
if( _joint == null ) {
CreateJoint();
} else {
_joint = PhysicsJoint.Create(_joint.State);
}
if( _base == null ) CreateBase();
if( _pend == null ) CreatePendulum();

// initialize children
_base.Parent = this;
_pend.Parent = this;

// connect joint
_joint.State.Connectors[0] = new EntityJointConnector(
_base,
new Vector3(1, 0, 0),
new Vector3(0, 1, 0),
new Vector3(0f, 0f, 0f)
);
_joint.State.Connectors[1] = new EntityJointConnector(
_pend,
new Vector3(1, 0, 0),
new Vector3(0, 1, 0),
new Vector3(0f, 0f, 0f)
);

// initialize will load the graphics mesh
base.Initialize(device, physicsEngine);
_base.Initialize(device, physicsEngine);
_pend.Initialize(device, physicsEngine);

_base.PhysicsEntity.IsKinematic = true;

//State.PhysicsPrimitives.Add(_base.BoxShape);
//State.PhysicsPrimitives.Add(_pend.BoxShape);
//CreateAndInsertPhysicsEntity(physicsEngine);

physicsEngine.InsertJoint(_joint);
//Flags |= VisualEntityProperties.DoCompletePhysicsShapeUpdate;
} catch (Exception ex) {
HasBeenInitialized = false;
InitError = ex.ToString();
}
}
public override void Update(FrameUpdate update) {
base.Update(update);
_base.Update(update);
_pend.Update(update);
}
public override void Render(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, MatrixTransforms transforms, CameraEntity currentCamera) {
base.Render(device, transforms, currentCamera);
_base.Render(device, transforms, currentCamera);
_pend.Render(device, transforms, currentCamera);
}
public override void Dispose() {
_base.Dispose();
_pend.Dispose();
base.Dispose();
}
}



Re: Microsoft Robotics - Simulation A simple jointed object

Eric Fritzinger

Relative position: If I change the positions of the parent, the child objects do not reposition.

I have fixed this in my other post in "Documentation on Pose", I need to post a reply to note the update.

Try using Children.Add(_base) instead of _base.Parent = this;

I'll see what I can do about the rest of the stuff here.

One thing, I noticed you did not create a PhysicsEntity for the VisualEntity. I have noticed problems with some things if I did not create a physics entity for my object.

Extra object: An extra object is added by the pendulum. It looks like a small sphere and I suspect it is the parent object. How do I get rid of this

I think you might be referring to the small sphere that starts at (0,0.1,0) in the world. That is the rigid body camera physics primitive. When you select that option from the Physics->Settings... menu, a small sphere is placed in front of the camera. The sphere you're seeing I believe is the sphere placed in front of the camera when "rigid body camera" is selected. That sphere is already in the world by default, you can ignore it.

The JointConnectors parameters do not make sense to me. I just played with them until I got the result I wanted. Why are the joint axises in a different orientation than the rest of the axises

I am not 100% myself on the usage of the "axis" parameter in the EntityJointConnector object, however the orientation allows you to rotate the joint to be along whichever axis you want. If you play with the orientation, you'll find some of the interesting things you can do with joints.

NOTE: You cannot have the "orientation" and "axis" parameters be the exact same. If they are the same, the joint creation will fail.

The last parameter of the EntityJointConnector is the offset from the specified entity where it connects to the joint.

Odd initial position: The child objects start out in weird places and do not go to the correct postion until I start the physics engine. Why is this Also, if I try to change the child positions in the editor the scale is off by a factor of 10.

I'm not sure about most of this, but I have noted that generally, unless you're very careful with your numbers, the physics engine will have to make corrections.

Hehehe, I should probably write a starting out tutorial on making VisualEntity objects, especially regarding joints. I went through so much headache trying to figure this stuff out.





Re: Microsoft Robotics - Simulation A simple jointed object

Alan Oursland

Based on Eric's suggestions, I modified my pendulum so that the support beam is a parent and the pendulum is a child. I call Children.Add(_pend) to add the pendulum object, and I do not call _pend.Initialize.

Now the support beam renders, but the pendulum still does not render. The positions are messed up though. If I add the pendulum to position (0,1,0) and look at th combined rendering view, the rendered image of the support beam is above the physics image of the support beam.

I have tried several of the things done in entities.cs, like calling CreateAndInsertPhysicalEntity. At best I get two physics objects added, but none of them have actually rendered.

The physics of the pendulum are still correct.

I also discovered that the extra object I was seeing is present in the simulation when it starts. You can see it if you turn on the physics view and look at (0,0,0). Does anyone know what that object is

I've attached the source code again. I am hoping someone can tell me how to get the pendulum to render (I took out the Render override that calls _pend.Render since it did not seem to do anything).

Code Snippet

[DataContract]
[CLSCompliant(true)]
public class SimplePendulum : SingleShapeEntity {
private PhysicsJoint _joint = null;
private SingleShapeEntity _base;
private SingleShapeEntity _pend;
[DataMember]
public Joint Joint {
get { return _joint; }
set { _joint = (PhysicsJoint)value; }
}
[DataMember]
public SingleShapeEntity Base {
get { return _base; }
set { _base = value; }
}
[DataMember]
public SingleShapeEntity Pendulum {
get { return _pend; }
set { _pend = value; }
}
private const float radius = 0.05f;
private const float length = 0.50f;
private const float mass = 10.0f;
public SimplePendulum() : this(new Vector3(0,0,0)) {
}
public SimplePendulum(Vector3 position) :
base(
new BoxShape( new BoxShapeProperties("base", mass, new Pose(position), new Vector3(radius, radius, length))),
position
) {
}
private void CreateJoint() {
JointAngularProperties commonAngular = new JointAngularProperties();
commonAngular.Swing2Mode = JointDOFMode.Free;
_joint = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));
_joint.State.Name = "Joint";
}
private void CreateBase() {
_base = this;
}
private void CreatePendulum() {
BoxShapeProperties prop = new BoxShapeProperties(
"child",
mass,
new Pose(new Vector3(0f, -length/2 + radius/2, -radius/2 - length/2)),

new Vector3(radius, length, radius));
Shape shape = new BoxShape(prop);
_pend = new SingleShapeEntity(shape, prop.LocalPose.Position);
}
public override void Initialize(xnagfx.GraphicsDevice device, PhysicsEngine physicsEngine) {
try {
InitError = string.Empty;
// programmatically build articulated arm
if( _joint == null ) {
CreateJoint();
} else {
_joint = PhysicsJoint.Create(_joint.State);
}
if( _base == null ) CreateBase();
if( _pend == null ) CreatePendulum();

// initialize children
Children.Add(_pend);

// connect joint
_joint.State.Connectors[0] = new EntityJointConnector(
_base,
new Vector3(1, 0, 0),
new Vector3(0, 1, 0),
new Vector3(0f, 0f, -length/2)
);
_joint.State.Connectors[1] = new EntityJointConnector(
_pend,
new Vector3(1, 0, 0),
new Vector3(0, 1, 0),
new Vector3(0f, 0f, -length/2)
);

// initialize will load the graphics mesh
base.Initialize(device, physicsEngine);

_base.PhysicsEntity.IsKinematic = true;
_pend.PhysicsEntity.IsKinematic = false;

physicsEngine.InsertJoint(_joint);
Flags |= VisualEntityProperties.DoCompletePhysicsShapeUpdate;
} catch (Exception ex) {
HasBeenInitialized = false;
InitError = ex.ToString();
}
}
}





Re: Microsoft Robotics - Simulation A simple jointed object

Alan Oursland

Ok, I'm getting closer.

On the pose thread, someone said that you have to considered the physics shapes as children and children poses are always relative to the parent. Child entities added with Children.Add show up as a tree in the simulator editor but the shapes display as properties so this was not quite clear.

I set the positions of both shapes to (0,0,0). Then I used the Position property on each entity to set the location (well, really just the child).

I also realized that my child entity needs a Name on its State. I had only been setting the Shape Name.

After I did this both entities render, the physics objects line up with the rendering, the support beam is fixed and the pendulum swings.

But this only works correctly if I position the pendulum at (0,0,0). If I position it to (0,1,0) then the pendulum entity is too high (although the physic shape is in the correct position). I'm still doing something wrong, but I don't know if it is in the EntityJointConnector or something else.

I've attached the source again.

Code Snippet

[DataContract]
[CLSCompliant(true)]
public class SimplePendulum : SingleShapeEntity {
private PhysicsJoint _joint = null;
private SingleShapeEntity _support;
private SingleShapeEntity _pend;
[DataMember]
public Joint Joint {
get { return _joint; }
set { _joint = (PhysicsJoint)value; }
}
[DataMember]
public SingleShapeEntity Support {
get { return _support; }
set { _support = value; }
}
[DataMember]
public SingleShapeEntity Pendulum {
get { return _pend; }
set { _pend = value; }
}
private const float radius = 0.05f;
private const float length = 0.50f;
private const float mass = 10.0f;
public SimplePendulum() : this(new Vector3(0,0,0)) {
}
public SimplePendulum(Vector3 position)
: base(
new BoxShape( new BoxShapeProperties("support:box", mass, new Pose(new Vector3(0,0,0)), new Vector3(radius, radius, length))),
position
) {
}
private void CreateJoint() {
JointAngularProperties commonAngular = new JointAngularProperties();
commonAngular.Swing2Mode = JointDOFMode.Free;
_joint = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));
_joint.State.Name = "Joint";
}
private void CreateSupport() {
_support = this;
}
private void CreatePendulum() {
BoxShapeProperties prop = new BoxShapeProperties(
"pendulum:box",
mass,
new Pose(new Vector3(0,0,0)),
new Vector3(radius, length, radius));
Shape shape = new BoxShape(prop);
_pend = new SingleShapeEntity(shape, prop.LocalPose.Position);
_pend.State.Name = "pendulum";
_pend.Position = new xna.Vector3(0f, -length/2 + radius/2, -radius/2 - length/2);
}
public override void Initialize(xnagfx.GraphicsDevice device, PhysicsEngine physicsEngine) {
try {
InitError = string.Empty;
// programmatically build articulated arm
if( _joint == null ) {
CreateJoint();
} else {
_joint = PhysicsJoint.Create(_joint.State);
}
if( _support == null ) CreateSupport();
if( _pend == null ) CreatePendulum();

// initialize children
Children.Add(_pend);

// connect joint
_joint.State.Connectors[0] = new EntityJointConnector(
_support,
new Vector3(1, 0, 0),
new Vector3(0, 1, 0),
new Vector3(0f, 0f, -length/2)
);
_joint.State.Connectors[1] = new EntityJointConnector(
_pend,
new Vector3(1, 0, 0),
new Vector3(0, 1, 0),
new Vector3(0f, length/2-radius/2, radius/2)
);

// initialize will load the graphics mesh
base.Initialize(device, physicsEngine);

_support.PhysicsEntity.IsKinematic = true;
_pend.PhysicsEntity.IsKinematic = false;

physicsEngine.InsertJoint(_joint);
Flags |= VisualEntityProperties.DoCompletePhysicsShapeUpdate;
} catch (Exception ex) {
HasBeenInitialized = false;
InitError = ex.ToString();
}
}
}





Re: Microsoft Robotics - Simulation A simple jointed object

Eric Fritzinger

What do you mean by the pendulum is too high Also, am I to assume that the physics primitive and visual rendering are completely matched up, now

I also discovered that the extra object I was seeing is present in the simulation when it starts. You can see it if you turn on the physics view and look at (0,0,0). Does anyone know what that object is


I've mentioned this earlier, and I know exactly what you're talking about. This is the sphere that appears in front of the camera when you select the "Enable rigid body camera for default camera" option in the Physics->Settings... menu.
Try this:
Start a blank world with just the ground, then switch the rendering option to "Combined" so that you can see both the physics primitives and the meshes. Now, if you look, you'll see that object at (0,0,0), even though no other objects are supposed to be present. Now select the "Enable rigid body camera for default camera" option. You'll notice the little sphere disappeared and you now have a little sphere in front of the camera (it actually zooms in a straight line to be in front of the camera, colliding with all objects in the way).

If after you've tried this and what I mentioned above did not happen, then there is definitely something going on that shouldn't be happening.




Re: Microsoft Robotics - Simulation A simple jointed object

Alan Oursland

You were right and I did not believe you. The extra ball is the camera and it sits on the ground unless you have the rigid camera body turned on. Very odd.

When I add the SimplePendulum entity at position (0,1,0) everything shows up in the right place. When I start the simulator, the pendulum (rendering and physics shape) moves up to somewhere near (0,2,0). The physics primitives and the visual rendering are still matched up correctly. The support beam stays where it is. I am assuming this is being caused by the joint.

I've been guessing that the joint connector positions are with respect to the entities to which it is being attached. I may be wrong about that.





Re: Microsoft Robotics - Simulation A simple jointed object

Eric Fritzinger

I agree, it is odd to have the little sphere right in the middle. I was a bit confused by it at first, but just learned to get around it by not putting anything in contact with (0,0,0).

You are also correct in that the joint is what is shifting your objects up. If there is something intersecting them or if there is a problem with the initial coordinates of the objects and the joint, then the physics engine will attempt to correct the problem, often resulting in sudden, jerking motions to get to a state of equilibrium.

So, below is an explanation of the EntityJointConnector stuff, for anyone unfamiliar with joints and is reading this thread.

Each joint has two connectors. It is represented by an array of length 2 under the Connectors property of PhysicsJoint.State. The EntityJointConnector object takes several parameters, listed below:

object entity - The entity you are connecting to the joint. It must be an entity type, not a shape type (BoxShape, CapsuleShape, SphereShape, etc... all incorrect).
Vector3 orientation - The orientation of the axis (the "which way is up " property) - I normally set this to (0,1,0) in order to follow the world's orientation.
Vector3 axis - Changes the direction of the joint based on the orientation
Vector3 connectPoint - This is a key property. This provides a relative position from the entity where the entity connects to the joint.

The connectPoint property is key when setting up the joint. As I mentioned it provides a relative position from the entity specified to where the entity connects to the joint. For the example below, assume a joint orientation of (0,1,0), as that does matter when creating a joint connectPoint.

Say you have a box at (0, 5, 0) (identified by box1 with dimensions (1,1,1)), and it's kinematic. This is a SingleShapeEntity. Let us say you have another SingleShapeEntity, box2, that you want to connect to box1. I set my joint up in the following way:

physicsJoint.State.Connectors[0] = new EntityJointConnector(box1, new Vector3(0,1,0), new Vector3(1,0,0), new Vector(0,0,0));
physicsJoint.State.Connectors[1] = new EntityJointConnector(box2, new Vector3(0,1,0), new Vector3(1,0,0), new Vector(0,0,0));

Essentially, it will create an effect that looks like this (* is where the joint is located):
_____
| * | box1/box2 - occupying same space
-----

^
|
Y
X->

This will create a joint between the two objects, box1 and box2, and box1 will connect to the joint through its center ((0,0,0) relative to box1) and box2 will also connect to the joint through its center ((0,0,0) relative to box2). What this will do is no matter where you initially position those entities, it will slam them together and will became, essentially, one object (the physics engine making corrections to follow the joint rules).

So, if you want them to be connected to where, let's say, box1 will connect to the joint at the bottom of box1 and box2 will connect to the bottom of box1, but will remain 3 meters below box1 (much akin to the pendulum problem presented here), then you will need to set them up like this (remember, the dimensions of each box is (1,1,1)):

physicsJoint.State.Connectors[0] = new EntityJointConnector(box1, new Vector3(0,1,0), new Vector3(1,0,0), new Vector(0,-0.5f,0));
physicsJoint.State.Connectors[1] = new EntityJointConnector(box2, new Vector3(0,1,0), new Vector3(1,0,0), new Vector(0,3.5f,0));

(connectPoint for box 1 = -0.5 = 1/2 box1's height, and connectPoint for box2 = 3.5 = 3 meters + 1/2 box2's height)
Now, the scene will look like this (* is where the joint is located):
_____
| | box1
--*-- ^
| |
| Y
__|__ X->
| | box2
-----

And now, depending on what freedom you gave the joint, you can swing or twist the objects and start having some fun.

The connectPoints that you give are relative to the entities that are provided in that connector. So, in the case of box2, the connectPoint was (0, 3.5f, 0), which means that the joint would be connected 3.5 meteres above box2. And also remember that an entity's position is determined by its center of mass (the exact middle of the entity), not a corner of the entity.

We can even do this
(* is where the joint is located):

physicsJoint.State.Connectors[0] = new EntityJointConnector(box1, new Vector3(0,1,0), new Vector3(1,0,0), new Vector(-2.5f,0,0));
physicsJoint.State.Connectors[1] = new EntityJointConnector(box2, new Vector3(0,1,0), new Vector3(1,0,0), new Vector(0,3.5f,0));

_____
*--| | box1
| ----- ^

| |
__|__ Y
| | box2 X->
-----

And now the joint is shifted to the left 2.5 meters from box1's center rather than down 1/2 box1's height.

So, in the second example, let's say that I intially positioned box1 at (0,5,0) and box2 at (1, 10, 4). box1 is kinematic, therefore it cannot be moved. We have the joint set up as it is in the second example. According to physics, we cannot possibly connect the joint given box1's and box2's positions. Since box1 cannot be moved, we can only assume that box2 must be moved to make the joint feasible, and so, the physics engine will auto-correct the problem by shifting box2 to position (0, 1, 0), which is the position for box2 that the joint between box1 and box2 can exist. The physics engine auto-corrects when it comes to joints, so having joints is a big help when determining positions of connected entities.

LinearJoints are another beast in and of themselves, but if anyone's interested in them, I can help with that, too =).

If anyone spots any errors in this, please let me know. I hope this has helped someone! =)





Re: Microsoft Robotics - Simulation A simple jointed object

Alan Oursland

Thank you, Eric. That is what I thought the joint connecter positions were. I had not considered the possibility that the 'normal' vector was 'up'. I kept thinking it meant normal to the angle which did not have a clear meaning to me.

There seems to be some issue with the joint position when some of the items are children. I think I have my joint set correctly, but the entity moves far off of where I specified. I'll keep playing with it and see if I get anywhere.





Re: Microsoft Robotics - Simulation A simple jointed object

Alan Oursland

I think there may be a problem with joints on child objects. The Kuka arm does not use child objects. It also cannot be repositioned by setting the parent position.



Re: Microsoft Robotics - Simulation A simple jointed object

Alan Oursland

Yep. I went back to the model used by the Kuka arm where the entities connected to the joint are not children of the root entity. Now I can create the pendulum with the rendering and the physics objects in the same (and correct) position.

I think joints are broken for child objects. Their real positions are dependent on the position of the parent and the joint does not know to take this into account.

The problem in my original code was that I was not setting the names of the entities (just the entity shapes). I also have to offset my pendulum positions by the parent position passed in at creation. The overriding of Render, Update, and Dispose are now required since the parent/child relationship does not exist.

I've attached my working code:

Code Snippet

[DataContract]
[CLSCompliant(true)]
public class SimplePendulum : VisualEntity {
private PhysicsJoint _joint = null;
private SingleShapeEntity _support;
private SingleShapeEntity _pend;
private Vector3 _parentPosition;
[DataMember]
public Joint Joint {
get { return _joint; }
set { _joint = (PhysicsJoint)value; }
}
[DataMember]
public SingleShapeEntity Support {
get { return _support; }
set { _support = value; }
}
[DataMember]
public SingleShapeEntity Pendulum {
get { return _pend; }
set { _pend = value; }
}
private const float radius = 0.05f;
private const float length = 0.50f;
private const float mass = 10.0f;
public SimplePendulum() {
}
public SimplePendulum(Vector3 initialPosition) {
base.State.Pose.Position = initialPosition;
}
private void CreateJoint() {
JointAngularProperties commonAngular = new JointAngularProperties();
commonAngular.TwistMode = JointDOFMode.Free;
_joint = PhysicsJoint.Create(new JointProperties(commonAngular, null, null));
_joint.State.Name = "Joint";
}
private void CreateSupport(Vector3 pos) {
BoxShapeProperties prop = new BoxShapeProperties(
"supportbox",
mass,
new Pose(new Vector3(0, 0, 0)),
new Vector3(radius, radius, length));
Shape shape = new BoxShape(prop);
_support = new SingleShapeEntity(shape, base.State.Pose.Position);
_support.State.Name = "support";
_support.State.Pose.Position = new Vector3(pos.X+0f, pos.Y+0f, pos.Z+0f);
}
private void CreatePendulum(Vector3 pos) {
BoxShapeProperties prop = new BoxShapeProperties(
"pendulumbox",
mass,
new Pose(new Vector3(0,0,0)),
new Vector3(radius, length, radius));
Shape shape = new BoxShape(prop);
_pend = new SingleShapeEntity(shape, base.State.Pose.Position);
_pend.State.Name = "pendulum";
_pend.State.Pose.Position = new Vector3(pos.X+0f, pos.Y-length/2 + radius/2, pos.Z-radius/2 - length/2);
}
public override void Initialize(xnagfx.GraphicsDevice device, PhysicsEngine physicsEngine) {
try {
InitError = string.Empty;

// connect joint
if (_joint == null) {
CreateJoint();
} else {
_joint = PhysicsJoint.Create(_joint.State);
}

if (_support == null) CreateSupport(base.State.Pose.Position);
if (_pend == null) CreatePendulum(base.State.Pose.Position);

// initialize will load the graphics mesh
base.Initialize(device, physicsEngine);
_support.Initialize(device, physicsEngine);
_pend.Initialize(device, physicsEngine);

_joint.State.Connectors[0] = new EntityJointConnector(
_support,
new Vector3(0, 1, 0),
new Vector3(0, 0, 1),
new Vector3(0f, 0f, -length/2)
);
_joint.State.Connectors[1] = new EntityJointConnector(
_pend,
new Vector3(0, 1, 0),
new Vector3(0, 0, 1),
new Vector3(0f, length/2-radius/2, radius/2)
);

_support.PhysicsEntity.IsKinematic = true;

PhysicsEngine.InsertJoint(_joint);
Flags |= VisualEntityProperties.DoCompletePhysicsShapeUpdate;
} catch (Exception ex) {
HasBeenInitialized = false;
InitError = ex.ToString();
}
}

public override void Update(FrameUpdate update) {
base.Update(update);
_support.Update(update);
_pend.Update(update);
}

public override void Render(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, MatrixTransforms transforms, CameraEntity currentCamera) {
base.Render(device, transforms, currentCamera);
_support.Render(device, transforms, currentCamera);
_pend.Render(device, transforms, currentCamera);
}

public override void Dispose() {
base.Dispose();
_support.Dispose();
_pend.Dispose();
}
}





Re: Microsoft Robotics - Simulation A simple jointed object

Alan Oursland

About the JointAngularProperties:

The first vector corresponds to the Y axis of the joint.

The second vector corresponds to the X axis of the joint.

You can see these axises in the physics view of the simulator.

The Twist degree of freedom is around the X axis.

The Swing1 and Swing2 DOF's are around the Y and Z axises but I am not sure which corresponds to which. It should be simple to determine if you are interested.





Re: Microsoft Robotics - Simulation A simple jointed object

George Chrysanthakopoulos

Alan, we will look into this, if having children messes up joints. But one thing to note about Children: The simulation engine will call their Update and Redner routines, AFTER calling the parent, and it will pass the world, view transforms after they have been modified by the parent. This means the children will have a wierd view of the world. Normally this is ok because children are meant ot be relative to the parent.

But in some cases, like with joints, the children might want a "pristine" world matrix (identity) so they will need to either adjust for that, or apply some extra transforms.





Re: Microsoft Robotics - Simulation A simple jointed object

Alan Oursland

Thanks George.

The solution that works for me is to have the member entities not be added as children like the Kuka arm implentation does. I call Initialize explicitly on each entity I create. I know I have to call Render on each entity in order for it to be visible. If I do not call Render then I only get the physics view objects.. I did not test Update to see if that is actually required, but since the Kuka arm does it I did it too.

If you add the Kuka arm with the physics engine paused, you will notice that all of the links are added at the position passed into the constructor. When the simulation runs the joints force the links into the correct positions. All of the link posititions are in world coordinates and not in the coordinate system of the container class.





Re: Microsoft Robotics - Simulation A simple jointed object

Eric Fritzinger

That's good to know about the issue with children entities. So far, I've been so careful with the calculation of the position of the children objects from the parent object that I haven't had any problems with them. Thanks for the heads up =).