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()));
        }
Exemple #2
0
        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));
        }
Exemple #3
0
        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");
     }
 }
Exemple #6
0
        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);
 }
Exemple #8
0
 /// <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));
 }
Exemple #9
0
 public LegPositions Rotate(Angle angle, LegFlags legs = LegFlags.All)
 {
     return(Rotate(Quaternion.CreateFromAxisAngle(Vector3.UnitZ, ((float)angle).DegreeToRad()), legs));
 }