public RobotDynamics(string name = DEFNAME) : base(name)
{
l = 0.2; //z
h = 0.05; //y
w = 0.1; //x
Body.Mass.Value = 0.5;
Body.Name = "Body";
AddChild(Body);
Body.SynchMeAfter += SynchWheelsToBodyPos;
FloorForce = new ForceCenter(1, new Position3D(0, 1, 0), null);
Body.AddForce(FloorForce);
SynchMassGeometry();
for (int i = 0; i < 4; i++)
{
wheels.Add(RbWheel.GetStandart());
}
wheels[0].localPos = new Vector3D(w / 2, 0, l / 2);
wheels[1].localPos = new Vector3D(w / 2, 0, -l / 2);
wheels[2].localPos = new Vector3D(-w / 2, 0, l / 2);
wheels[3].localPos = new Vector3D(-w / 2, 0, -l / 2);
//double moment = 0.1;
//foreach(var w in wheels) {
// Body.AddChild(w);
// w.Mx = moment;
// moment += moment;
//}
}
public static RbWheel GetStandart(Vector3D localPos)
{
int n = 11;
double r_real = 0.015;
double R_ideal = 0.009 / (2 * Sin(PI / n));
var mass = PI * r_real * r_real * 0.021 * 1080;
var ix = mass * r_real * r_real / 2;
var res = new RbWheel(n, R_ideal, R_ideal + 0.002, 0.01, 0.009 / 4)
{
Mass = 0.002242,
Ix = ix
};
res.localPos = localPos;
return(res);
}