添加单车底盘仿真平台并完善运动控制与夹臂功能
This commit is contained in:
@@ -0,0 +1,382 @@
|
||||
using MyParking.Shared;
|
||||
using MyParking.Simulation.Models;
|
||||
|
||||
namespace MyParking.Simulation.Core;
|
||||
|
||||
/// <summary>
|
||||
/// 保存单辆四舵轮停车机器人的离线仿真状态。
|
||||
/// </summary>
|
||||
public sealed class SimulationVehicle
|
||||
{
|
||||
public const double BodyLengthMeters = 1.472;
|
||||
public const double BodyWidthMeters = 0.948;
|
||||
|
||||
// 当前MDCSToolBox.dll的MultiWheelChassisInitializer运行时轮位：
|
||||
// X=±750mm、Y=±500mm，舵角机械限位为±120°。
|
||||
private const double WheelX = 0.75;
|
||||
private const double WheelY = 0.5;
|
||||
private const double MaximumBodyAcceleration = 0.6;
|
||||
private const double MaximumAngularAcceleration = 0.8;
|
||||
|
||||
private readonly List<VirtualSteerWheel> _wheels;
|
||||
private readonly double _initialX;
|
||||
private readonly double _initialY;
|
||||
private readonly double _initialYaw;
|
||||
|
||||
private Twist2D _targetBodyTwist = Twist2D.Zero;
|
||||
private double _actualVx;
|
||||
private double _actualVy;
|
||||
private double _actualOmega;
|
||||
|
||||
public SimulationVehicle(
|
||||
int vehicleId,
|
||||
double initialX,
|
||||
double initialY,
|
||||
double initialYaw)
|
||||
{
|
||||
VehicleId = vehicleId;
|
||||
_initialX = initialX;
|
||||
_initialY = initialY;
|
||||
_initialYaw = initialYaw;
|
||||
|
||||
_wheels =
|
||||
[
|
||||
new VirtualSteerWheel("左前", WheelX, WheelY),
|
||||
new VirtualSteerWheel("右前", WheelX, -WheelY),
|
||||
new VirtualSteerWheel("左后", -WheelX, WheelY),
|
||||
new VirtualSteerWheel("右后", -WheelX, -WheelY)
|
||||
];
|
||||
|
||||
Reset();
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public string Mode { get; private set; } = "Normal";
|
||||
|
||||
public double XMeters { get; private set; }
|
||||
|
||||
public double YMeters { get; private set; }
|
||||
|
||||
public double YawRadians { get; private set; }
|
||||
|
||||
public bool ModeReady => _wheels.All(wheel => wheel.IsAligned);
|
||||
|
||||
/// <summary>
|
||||
/// 停车并切换舵轮准备模式。
|
||||
/// </summary>
|
||||
public bool SetMode(string mode)
|
||||
{
|
||||
Stop();
|
||||
|
||||
var success = mode switch
|
||||
{
|
||||
"Normal" => PrepareParallelDirection(0.0),
|
||||
"CrabLeft" => PrepareParallelDirection(90.0),
|
||||
"CrabRight" => PrepareParallelDirection(-90.0),
|
||||
"Spin" => PrepareSpinDirection(),
|
||||
_ => false
|
||||
};
|
||||
|
||||
if (success)
|
||||
Mode = mode;
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按当前模式发送前进或后退命令。
|
||||
/// </summary>
|
||||
public bool Move(double directionSign)
|
||||
{
|
||||
if (!ModeReady)
|
||||
return false;
|
||||
|
||||
const double linearSpeed = 0.35;
|
||||
const double angularSpeed = 0.45;
|
||||
|
||||
var command = Mode switch
|
||||
{
|
||||
"Normal" => new Twist2D(
|
||||
directionSign * linearSpeed, 0.0, 0.0),
|
||||
"CrabLeft" => new Twist2D(
|
||||
0.0, directionSign * linearSpeed, 0.0),
|
||||
"CrabRight" => new Twist2D(
|
||||
0.0, -directionSign * linearSpeed, 0.0),
|
||||
"Spin" => new Twist2D(
|
||||
0.0, 0.0, directionSign * angularSpeed),
|
||||
_ => Twist2D.Zero
|
||||
};
|
||||
|
||||
return ApplyCommand(
|
||||
new ChassisCommand(VehicleId, command));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在当前运动模式下增加逆时针或顺时针转动。
|
||||
/// </summary>
|
||||
public bool Turn(double directionSign)
|
||||
{
|
||||
if (!ModeReady || Mode == "Spin")
|
||||
return false;
|
||||
|
||||
var command = new Twist2D(
|
||||
_targetBodyTwist.VxMetersPerSecond,
|
||||
_targetBodyTwist.VyMetersPerSecond,
|
||||
directionSign * 0.28);
|
||||
|
||||
return ApplyCommand(
|
||||
new ChassisCommand(VehicleId, command));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将网页虚拟遥控器的油门和转向组合为连续车体速度命令。
|
||||
/// </summary>
|
||||
public bool ManualDrive(
|
||||
double throttle,
|
||||
double steering,
|
||||
double speedScale,
|
||||
double steeringScale)
|
||||
{
|
||||
if (!AreFinite(
|
||||
throttle,
|
||||
steering,
|
||||
speedScale,
|
||||
steeringScale))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
throttle = Math.Clamp(throttle, -1.0, 1.0);
|
||||
steering = Math.Clamp(steering, -1.0, 1.0);
|
||||
speedScale = Math.Clamp(speedScale, 0.0, 1.0);
|
||||
steeringScale = Math.Clamp(steeringScale, 0.0, 1.0);
|
||||
|
||||
if (Math.Abs(throttle) < 0.001 &&
|
||||
Math.Abs(steering) < 0.001)
|
||||
{
|
||||
Stop();
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!ModeReady)
|
||||
return false;
|
||||
|
||||
const double maximumLinearSpeed = 0.6;
|
||||
const double maximumAngularSpeed = 0.7;
|
||||
|
||||
var linearSpeed =
|
||||
throttle * maximumLinearSpeed * speedScale;
|
||||
var angularSpeed =
|
||||
steering * maximumAngularSpeed * steeringScale;
|
||||
|
||||
var twist = Mode switch
|
||||
{
|
||||
"Normal" => new Twist2D(
|
||||
linearSpeed,
|
||||
0.0,
|
||||
angularSpeed),
|
||||
"CrabLeft" => new Twist2D(
|
||||
0.0,
|
||||
linearSpeed,
|
||||
angularSpeed),
|
||||
"CrabRight" => new Twist2D(
|
||||
0.0,
|
||||
-linearSpeed,
|
||||
angularSpeed),
|
||||
"Spin" => new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
angularSpeed),
|
||||
_ => Twist2D.Zero
|
||||
};
|
||||
|
||||
return ApplyCommand(
|
||||
new ChassisCommand(VehicleId, twist));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 应用统一车体速度命令并分解为四个舵轮速度向量。
|
||||
/// </summary>
|
||||
public bool ApplyCommand(ChassisCommand command)
|
||||
{
|
||||
if (command.VehicleId != VehicleId)
|
||||
return false;
|
||||
|
||||
var twist = command.BodyTwist;
|
||||
var wheelCommands = _wheels.Select(wheel =>
|
||||
{
|
||||
var wheelVx =
|
||||
twist.VxMetersPerSecond -
|
||||
twist.OmegaRadiansPerSecond * wheel.YMeters;
|
||||
|
||||
var wheelVy =
|
||||
twist.VyMetersPerSecond +
|
||||
twist.OmegaRadiansPerSecond * wheel.XMeters;
|
||||
|
||||
return (Wheel: wheel, Vx: wheelVx, Vy: wheelVy);
|
||||
}).ToArray();
|
||||
|
||||
foreach (var item in wheelCommands)
|
||||
{
|
||||
if (!item.Wheel.SetVelocityVector(
|
||||
item.Vx,
|
||||
item.Vy))
|
||||
{
|
||||
Stop();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
_targetBodyTwist = twist;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将车辆目标速度设置为零并保持当前舵轮角度。
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
_targetBodyTwist = Twist2D.Zero;
|
||||
foreach (var wheel in _wheels)
|
||||
wheel.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 更新舵轮反馈和车辆世界位姿。
|
||||
/// </summary>
|
||||
public void Step(double deltaTimeSeconds)
|
||||
{
|
||||
foreach (var wheel in _wheels)
|
||||
wheel.Step(deltaTimeSeconds);
|
||||
|
||||
var canMove = _wheels.All(wheel => wheel.IsAligned);
|
||||
var targetVx = canMove
|
||||
? _targetBodyTwist.VxMetersPerSecond
|
||||
: 0.0;
|
||||
var targetVy = canMove
|
||||
? _targetBodyTwist.VyMetersPerSecond
|
||||
: 0.0;
|
||||
var targetOmega = canMove
|
||||
? _targetBodyTwist.OmegaRadiansPerSecond
|
||||
: 0.0;
|
||||
|
||||
_actualVx = MoveTowards(
|
||||
_actualVx,
|
||||
targetVx,
|
||||
MaximumBodyAcceleration * deltaTimeSeconds);
|
||||
|
||||
_actualVy = MoveTowards(
|
||||
_actualVy,
|
||||
targetVy,
|
||||
MaximumBodyAcceleration * deltaTimeSeconds);
|
||||
|
||||
_actualOmega = MoveTowards(
|
||||
_actualOmega,
|
||||
targetOmega,
|
||||
MaximumAngularAcceleration * deltaTimeSeconds);
|
||||
|
||||
var cos = Math.Cos(YawRadians);
|
||||
var sin = Math.Sin(YawRadians);
|
||||
|
||||
var worldVx = cos * _actualVx - sin * _actualVy;
|
||||
var worldVy = sin * _actualVx + cos * _actualVy;
|
||||
|
||||
XMeters += worldVx * deltaTimeSeconds;
|
||||
YMeters += worldVy * deltaTimeSeconds;
|
||||
YawRadians = FrameTransform2D.NormalizeAngle(
|
||||
YawRadians + _actualOmega * deltaTimeSeconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 恢复车辆初始位置和舵轮状态。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
XMeters = _initialX;
|
||||
YMeters = _initialY;
|
||||
YawRadians = _initialYaw;
|
||||
Mode = "Normal";
|
||||
_targetBodyTwist = Twist2D.Zero;
|
||||
_actualVx = 0.0;
|
||||
_actualVy = 0.0;
|
||||
_actualOmega = 0.0;
|
||||
|
||||
foreach (var wheel in _wheels)
|
||||
wheel.Reset();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建供网页读取的不可变状态快照。
|
||||
/// </summary>
|
||||
public VehicleStateDto GetSnapshot()
|
||||
{
|
||||
return new VehicleStateDto(
|
||||
VehicleId,
|
||||
XMeters,
|
||||
YMeters,
|
||||
YawRadians,
|
||||
BodyLengthMeters,
|
||||
BodyWidthMeters,
|
||||
Mode,
|
||||
ModeReady,
|
||||
new TwistStateDto(
|
||||
_targetBodyTwist.VxMetersPerSecond,
|
||||
_targetBodyTwist.VyMetersPerSecond,
|
||||
_targetBodyTwist.OmegaRadiansPerSecond),
|
||||
new TwistStateDto(
|
||||
_actualVx,
|
||||
_actualVy,
|
||||
_actualOmega),
|
||||
_wheels.Select(wheel =>
|
||||
new WheelStateDto(
|
||||
wheel.Name,
|
||||
wheel.XMeters,
|
||||
wheel.YMeters,
|
||||
wheel.TargetAngleDegrees,
|
||||
wheel.ActualAngleDegrees,
|
||||
wheel.TargetSpeedMetersPerSecond,
|
||||
wheel.ActualSpeedMetersPerSecond,
|
||||
wheel.IsAligned)).ToArray());
|
||||
}
|
||||
|
||||
private bool PrepareParallelDirection(double targetAngleDegrees)
|
||||
{
|
||||
return _wheels.All(wheel =>
|
||||
wheel.PrepareDirection(targetAngleDegrees));
|
||||
}
|
||||
|
||||
private bool PrepareSpinDirection()
|
||||
{
|
||||
var success = true;
|
||||
|
||||
foreach (var wheel in _wheels)
|
||||
{
|
||||
var vx = -wheel.YMeters;
|
||||
var vy = wheel.XMeters;
|
||||
success &= wheel.SetVelocityVector(vx, vy);
|
||||
wheel.Stop();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
private static double MoveTowards(
|
||||
double current,
|
||||
double target,
|
||||
double maximumChange)
|
||||
{
|
||||
var difference = target - current;
|
||||
if (Math.Abs(difference) <= maximumChange)
|
||||
return target;
|
||||
|
||||
return current + Math.Sign(difference) * maximumChange;
|
||||
}
|
||||
|
||||
private static bool AreFinite(params double[] values)
|
||||
{
|
||||
return values.All(value =>
|
||||
!double.IsNaN(value) &&
|
||||
!double.IsInfinity(value));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user