private async Task ExecuteStep(Vector2 direction, float angle, LegFlags forwardMovingLegs, float distance = 1f, float liftHeight = 2)
{
_rotationalOdometry += angle;
angle = angle / 4;
if (forwardMovingLegs != LegFlags.LfRrCross && forwardMovingLegs != LegFlags.RfLrCross)
{
throw new ArgumentException($"{nameof(forwardMovingLegs)} has to be {nameof(LegFlags.RfLrCross)} or {nameof(LegFlags.LfRrCross)}");
}
LegFlags backwardsMovingLegs = forwardMovingLegs == LegFlags.LfRrCross ? LegFlags.RfLrCross : LegFlags.LfRrCross;
var legShift = distance / 4;
// Lift
var nextStep = RelaxedStance
.Transform(new Vector3(legShift * direction.X, legShift * direction.Y, liftHeight), forwardMovingLegs)
.Rotate(new Angle(-angle), forwardMovingLegs)
.Transform(new Vector3(-legShift * direction.X, -legShift * direction.Y, 0), backwardsMovingLegs)
.Rotate(new Angle(angle), backwardsMovingLegs);
await ExecuteMove(nextStep);
// Lower
nextStep = nextStep
.Transform(new Vector3(legShift * direction.X, legShift * direction.Y, -liftHeight), forwardMovingLegs)
.Rotate(new Angle(-angle), forwardMovingLegs)
.Transform(new Vector3(-legShift * direction.X, -legShift * direction.Y, 0), backwardsMovingLegs)
.Rotate(new Angle(angle), backwardsMovingLegs);
await ExecuteMove(nextStep);
// Move all
nextStep = nextStep.Transform(new Vector3(-legShift * direction.X * 2, -legShift * direction.Y * 2, 0));
await ExecuteMove(nextStep);
_odometry += Vector2.Transform(direction * distance, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, _rotationalOdometry.DegreeToRad()));
}
private LegPositions Rotate(Quaternion rotation, LegFlags legs = LegFlags.All)
{
Vector3 newLeftFront = LeftFront;
Vector3 newRightFront = RightFront;
Vector3 newLeftRear = LeftRear;
Vector3 newRightRear = RightRear;
if ((legs & LegFlags.LeftFront) != 0)
{
newLeftFront = Vector3.Transform(LeftFront, rotation);
}
if ((legs & LegFlags.RightFront) != 0)
{
newRightFront = Vector3.Transform(RightFront, rotation);
}
if ((legs & LegFlags.LeftRear) != 0)
{
newLeftRear = Vector3.Transform(LeftRear, rotation);
}
if ((legs & LegFlags.RightRear) != 0)
{
newRightRear = Vector3.Transform(RightRear, rotation);
}
return(new LegPositions(newLeftFront, newRightFront, newLeftRear, newRightRear));
}
public LegPositions Transform(Vector3 transformVector, LegFlags legs = LegFlags.All)
{
Vector3 newLeftFront = LeftFront;
Vector3 newRightFront = RightFront;
Vector3 newLeftRear = LeftRear;
Vector3 newRightRear = RightRear;
if ((legs & LegFlags.LeftFront) != 0)
{
newLeftFront = LeftFront + transformVector;
}
if ((legs & LegFlags.RightFront) != 0)
{
newRightFront = RightFront + transformVector;
}
if ((legs & LegFlags.LeftRear) != 0)
{
newLeftRear = LeftRear + transformVector;
}
if ((legs & LegFlags.RightRear) != 0)
{
newRightRear = RightRear + transformVector;
}
return(new LegPositions(newLeftFront, newRightFront, newLeftRear, newRightRear));
}
private async Task StepToRelaxed(LegFlags legsToAlign, float liftHeight = 2)
{
if (legsToAlign != LegFlags.LfRrCross && legsToAlign != LegFlags.RfLrCross)
{
throw new ArgumentException($"{nameof(legsToAlign)} has to be {nameof(LegFlags.RfLrCross)} or {nameof(LegFlags.LfRrCross)}");
}
await ExecuteMove(RelaxedStance
.Transform(new Vector3(0, 0, liftHeight), legsToAlign));
await ExecuteMove(RelaxedStance);
}
public void MoveLeg(Vector3 target, LegFlags legs = LegFlags.All)
{
if (legs == LegFlags.LeftFront)
{
MoveLeftFrontLeg(target);
}
if (legs == LegFlags.RightFront)
{
MoveRightFrontLeg(target);
}
if (legs == LegFlags.LeftRear)
{
MoveLeftRearLeg(target);
}
if (legs == LegFlags.RightRear)
{
MoveRightRearLeg(target);
}
else
{
throw new InvalidOperationException("Moving multiple legs to the same position is not advisable");
}
}
static void Main(string[] args)
{
Directory.SetCurrentDirectory(Path.GetDirectoryName(Assembly.GetEntryAssembly().Location));
Console.WriteLine("Starting");
using (var driver = new DynamixelDriver(args.Length > 0 ? args[0] : "COM4"))
using (var quadruped = new QuadrupedIkDriver(driver))
{
LoadLimits(driver);
using (var gaiteEngine = new InterpolationGaitEngine(quadruped))
using (var controller = new InterpolationController(gaiteEngine))
{
bool keepGoing = true;
LegFlags nextLegCombo = LegFlags.RfLrCross;
while (keepGoing)
{
nextLegCombo = nextLegCombo == LegFlags.RfLrCross ? LegFlags.LfRrCross : LegFlags.RfLrCross;
Vector2 direction = Vector2.Zero;
ConsoleKeyInfo keyInfo = GetCurrentConsoleKey();
switch (keyInfo.Key)
{
case ConsoleKey.LeftArrow:
case ConsoleKey.A:
direction = new Vector2(-1, 0);
break;
case ConsoleKey.RightArrow:
case ConsoleKey.D:
direction = new Vector2(1, 0);
break;
case ConsoleKey.UpArrow:
case ConsoleKey.W:
direction = new Vector2(0, 1);
break;
case ConsoleKey.DownArrow:
case ConsoleKey.S:
direction = new Vector2(0, -1);
break;
case ConsoleKey.D3:
controller.EnqueueOneRotation(keyInfo.Modifiers == ConsoleModifiers.Shift ? 25 : 12.5f, nextLegCombo);
break;
case ConsoleKey.D2:
controller.EnqueueOneRotation(keyInfo.Modifiers == ConsoleModifiers.Shift ? -25 : -12.5f, nextLegCombo);
break;
case ConsoleKey.Q:
direction = keyInfo.Modifiers == ConsoleModifiers.Shift ? new Vector2(-1, -1) : new Vector2(-1, 1);
break;
case ConsoleKey.E:
direction = keyInfo.Modifiers == ConsoleModifiers.Shift ? new Vector2(1, -1) : new Vector2(1, 1);
break;
case ConsoleKey.Z:
direction = new Vector2(-1, -1);
break;
case ConsoleKey.C:
direction = new Vector2(1, -1);
break;
case ConsoleKey.Spacebar:
controller.EnqueueOneRotation(0);
break;
case ConsoleKey.Escape:
keepGoing = false;
break;
}
controller.EnqueueOneStep(direction, nextLegCombo);
gaiteEngine.WaitUntilCommandQueueIsEmpty();
}
gaiteEngine.Stop();
quadruped.DisableMotors();
}
}
Console.WriteLine("Done");
}
private LegFlags GetNextLegCombo()
{
_lastUsedCombo = _lastUsedCombo == LegFlags.RfLrCross ? LegFlags.LfRrCross : LegFlags.RfLrCross;
return(_lastUsedCombo);
}
/// <summary>
/// Rotate the conter of the robot by degrees
/// </summary>
/// <param name="rotation"></param>
/// <param name="legs"></param>
public LegPositions Rotate(Rotation rotation, LegFlags legs = LegFlags.All)
{
return(Rotate(Quaternion.CreateFromYawPitchRoll(rotation.Pitch.DegreeToRad(), rotation.Roll.DegreeToRad(), rotation.Yaw.DegreeToRad()), legs));
}
public LegPositions Rotate(Angle angle, LegFlags legs = LegFlags.All)
{
return(Rotate(Quaternion.CreateFromAxisAngle(Vector3.UnitZ, ((float)angle).DegreeToRad()), legs));
}