More Joint Observations and Problems
I've created a new project with a horizontal rotating arm. The JointThingEntity has a Torque data member that can be set to change the torque applied during Update.
(Is there a better place to make this code public? I seem to remember something about uploading to channel9, but I can't find the appropriate place.)
Observations:
1.
The range of the arm angle is [-2PI .. +2PI]. The transition from positive to negative does not always go smoothly. Sometimes I get positions readings that look like this:
4.676, 5.020, 5.593, 6.051, 4.378, -5.942, -5.598, -5.369, -5.140
and
5.493, 5.951, 6.176, -5.966, -6.041, -5.812, -5.583
Am I calculating the angle incorrectly or is this a bug? It kind of looks like some of the Updates happen out of order, but since I am looking at the PhysicsEntity state I don't see how that is possible.
2.
Update does not seem to mesh perfectly with the simulator. Sometimes I get zero position change across time even though the arm is rotating. Any idea why this is happening?
I also get wildly fluctuating time elapsed values from the frame such as:
0.02609, 0.03818, 0.14455, 0.00294, 0.01756, 0.03034, 0.03076
I think this is resulting in much, but not all, of the noise.
3.
There seems to be some sort of non-linear friction. The steady state velocity for Torque=10 is different than the steady state velocity for Torque=1. If I set Torque=0 after it has reached the steady state velocity for Torque=10, it looks like the velocity exponentially decays to a nearly constant speed.
This implies that there is very little friction below 2 rad/s, but that the friction increases linearly above that speed.
How can I get rid of this friction? I would prefer to have complete control on how my physical entities are modelled. Did I accidently set something during my entity creation?
I've checked the following values to make sure there is no friction in the state.
entity.BoxShape.State.MassDensity.AngularDamping = 0
entity.BoxShape.State.MassDensity.LinearDamping = 0
entity.BoxShape.State.Material = null
4.
Measuring velocity is still very noisy. I would very much like a way to read velocity directly from the simulator or have values sent to Update that allow me to calculate velocity more accurately. Again, I want full control over how I model sensor error in my simulation entities.
#region Simulation namespaces namespace oursland.robotics.JointTest { public JointTestService(DsspServiceCreationPort creationPort) : base(creationPort) { }
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.ComponentModel;
using System.Xml;
using jointtest = oursland.robotics.JointTest;
using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
using System.ComponentModel;
using W3C.Soap;
#endregion
[DisplayName("JointTest")]
[Description("The JointTest Service")]
[Contract(Contract.Identifier)]
public class JointTestService : DsspServiceBase {
[Partner("Engine",
Contract = engineproxy.Contract.Identifier,
CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
private engineproxy.SimulationEnginePort _engineStub = new engineproxy.SimulationEnginePort();
private oursland.robotics.JointTest.JointThingEntity.JointThingEntity _thing;
[ServicePort("/jointtest", AllowMultipleInstances = false)]
private JointTestOperations _mainPort = new JointTestOperations();
}
protected override void Start() {
base.Start();
SetupCamera();
PopulateWorld();
}
private void SetupCamera() {
CameraView view = new CameraView();
view.EyePosition = new Vector3(0f, 1.5f, -1f);
view.LookAtPoint = new Vector3(0f, 0.5f, 0f);
SimulationEngine.GlobalInstancePort.Update(view);
}
private void PopulateWorld() {
AddLight();
AddJointThing();
}
void AddLight() {
LightSourceEntity sun = new LightSourceEntity();
sun.State.Name = "Sun";
sun.Type = LightSourceEntityType.Directional;
sun.Color = new Vector4(0.8f, 0.8f, 0.8f, 1);
sun.Direction = new Vector3(0.5f, -.75f, 0.5f);
SimulationEngine.GlobalInstancePort.Insert(sun);
}
void AddJointThing() {
_thing = new oursland.robotics.JointTest.JointThingEntity.JointThingEntity(new Vector3(0, 1, 0));
_thing.State.Name = "jointthing";
SimulationEngine.GlobalInstancePort.Insert(_thing);
}
}
public sealed class Contract {
public const String Identifier = "http://schemas.oursland.net/2007/06/jointtest.html";
}
public class JointTestOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop> {
}
[assembly: CLSCompliant(true)] [CLSCompliant(true)] [DataMember] public JointThingEntity() { public override void Initialize(xnagfx.GraphicsDevice device, PhysicsEngine physicsEngine) { // connect joint if (_support == null) CreateSupport(base.State.Pose.Position); // initialize will load the graphics mesh _pivotJoint.State.Connectors[0] = new EntityJointConnector( _support.PhysicsEntity.IsKinematic = true; _support.PhysicsEntity.SolverIterationCount = 64; PhysicsEngine.InsertJoint(_pivotJoint); public override void Update(FrameUpdate update) { Quaternion armRotation = entity.State.Pose.Orientation; float x = axisAngle.Angle * Vector3.Dot(axisAngle.Axis, jointAxis);
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
using xna = Microsoft.Xna.Framework;
using xnagfx = Microsoft.Xna.Framework.Graphics;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using System.ComponentModel;
namespace oursland.robotics.JointTest.JointThingEntity {
public class JointThingEntity : VisualEntity {
private PhysicsJoint _pivotJoint = null;
private SingleShapeEntity _support;
private SingleShapeEntity _swingArm;
private float _torque = 10.0f;
private const float radius = 0.05f;
private const float length = 0.50f;
private const float mass = 25.0f;
private double _lastX = float.NaN;
private double _lastT = double.NaN;
public float Torque {
get { return _torque; }
set { _torque = value; }
}
}
public JointThingEntity(Vector3 initialPosition) {
base.State.Pose.Position = initialPosition;
}
private void CreatePivotJoint() {
float damping = 0f;
Microsoft.Robotics.PhysicalModel.
JointDriveProperties jointDrive = new JointDriveProperties(
JointDriveMode.Position,
new SpringProperties(0, damping, 0),
100
);
JointAngularProperties jointProperties = new JointAngularProperties();
jointProperties.TwistMode = JointDOFMode.Free;
jointProperties.TwistDrive = jointDrive;
_pivotJoint = PhysicsJoint.Create(new JointProperties(jointProperties, null, null));
_pivotJoint.State.Name = "PivotJoint";
}
private void CreateSupport(Vector3 pos) {
BoxShapeProperties prop = new BoxShapeProperties(
"supportbox",
mass,
new Pose(new Vector3(0, 0, 0)),
new Vector3(radius, length, radius));
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 CreateSwingArm(Vector3 pos) {
BoxShapeProperties prop = new BoxShapeProperties(
"swingbox",
mass,
new Pose(new Vector3(0, 0, 0)),
new Vector3(radius, radius, length));
Shape shape = new BoxShape(prop);
_swingArm = new SingleShapeEntity(shape, base.State.Pose.Position);
_swingArm.State.Name = "swing";
_swingArm.State.Pose.Position = new Vector3(pos.X + 0f, pos.Y + length/2 + radius/2, pos.Z + length/2 - radius/2);
}
try {
InitError = string.Empty;
if (_pivotJoint == null) {
CreatePivotJoint();
} else {
_pivotJoint = PhysicsJoint.Create(_pivotJoint.State);
}
if (_swingArm == null) CreateSwingArm(base.State.Pose.Position);
base.Initialize(device, physicsEngine);
_support.Initialize(device, physicsEngine);
_swingArm.Initialize(device, physicsEngine);
_support,
new Vector3(1, 0, 0),
new Vector3(0, 1, 0),
new Vector3(0f, length/2, 0f)
);
_pivotJoint.State.Connectors[1] = new EntityJointConnector(
_swingArm,
new Vector3(1, 0, 0),
new Vector3(0, 1, 0),
new Vector3(0f, -radius/2, -length/2 + radius/2)
);
_swingArm.PhysicsEntity.SolverIterationCount = 64;
Flags |= VisualEntityProperties.DoCompletePhysicsShapeUpdate;
} catch (Exception ex) {
HasBeenInitialized = false;
InitError = ex.ToString();
}
}
base.Update(update);
_support.Update(update);
_swingArm.Update(update);
_swingArm.PhysicsEntity.ApplyTorque(new Vector3(0f, _torque, 0f));
float x = GetJointAngle(_pivotJoint, _swingArm);
double t = update.ElapsedTime;
if (_lastX != double.NaN && _lastT != double.NaN) {
double dx = GetAngleDifference(x, _lastX);
System.Console.WriteLine(x + " " + _lastX + " " + dx + " " +_lastT + " " + dx / _lastT);
}
_lastX = x;
_lastT = t;
}
private float GetJointAngle(PhysicsJoint joint, VisualEntity entity) {
Vector3 jointAxis = joint.State.Connectors[1].JointAxis;
AxisAngle axisAngle = Quaternion.ToAxisAngle(armRotation);
return x;
}
private double GetAngleDifference(double a1, double a2) {
double result = a1 - a2;
while (result > Math.PI) {
result -= 2 * Math.PI;
}
while (result < -Math.PI) {
result += 2 * Math.PI;
}
return result;
}
public override void Render(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, MatrixTransforms transforms, CameraEntity currentCamera) {
base.Render(device, transforms, currentCamera);
_support.Render(device, transforms, currentCamera);
_swingArm.Render(device, transforms, currentCamera);
}
public override void Dispose() {
base.Dispose();
_support.Dispose();
_swingArm.Dispose();
}
}
}
