Navigator, Pilot, PoseProvider, coordinate System questions

This is where you talk about the EV3 software itself, installation issues, and programming talk.

Moderators: roger, gloomyandy, skoehler

RoboRobo
Novice
Posts: 29
Joined: Sat Jun 18, 2016 2:10 pm

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby RoboRobo » Thu Jun 22, 2017 6:25 pm

Hi,

yep, I've tried (20, 0, 0) and (20, 20, 0) route. Let's start with my code:

Code: Select all

Wheel wheel1 = WheeledChassis.modelWheel(new EV3LargeRegulatedMotor(MotorPort.B), 4.32).offset(11.5).invert(true);
       Wheel wheel2 = WheeledChassis.modelWheel(new EV3LargeRegulatedMotor(MotorPort.D), 4.32).offset(-11.5).invert(true);
       Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 }, WheeledChassis.TYPE_DIFFERENTIAL);
      
       MovePilot pilot = new MovePilot(chassis);
      OdometryPoseProvider pp = new OdometryPoseProvider(pilot);
      
      
       pilot.setLinearSpeed(20);
         pilot.setAngularSpeed(50);
         
         Navigator navigator = new Navigator(pilot);
         
         Log l = new Log();
         
         navigator.goTo(20, 0, 0);
         navigator.waitForStop();
         
         LCD.clear(3);
         LCD.drawString("P " + pp.getPose(), 0, 3);
            
         Last_Pose = pp.getPose().toString();
         
         l.fix();
         
         navigator.goTo(20, 20, 0);
         navigator.waitForStop();
         
         LCD.clear(3);
      LCD.drawString("P " + pp.getPose(), 0, 3);
         
      Last_Pose = pp.getPose().toString();
         l.fix();
         



And here you may find results from pose provider of several trials:

X:20 Y:0 H:0
X:76 Y:0 H:0
----------------
X:20 Y:0 H:-0
X:60 Y:-0 H:-0
----------------
X:20 Y:0 H:0
X:0 Y:20 H:0
----------------
X:20 Y:0 H:0
X:-0 Y:20 H:0
----------------
X:20 Y:0 H:0
X:0 Y:20 H:0
----------------
X:20 Y:0 H:0
X:20 Y:0 H:-135
----------------
X:20 Y:0 H:-0
X:-0 Y:20 H:0
----------------

Robot also moved in strange trajectory, as it would be programmed to go to (20, 0, 0) and (0, 20, 0).
Now the most interesting notice: the navigator worked fine after I removed Pose Provider from the code, robot visually moved as it should. Could it be that Odometry Pose Provider somehow disturbs navigator's work?

Aswin
leJOS Team Member
Posts: 310
Joined: Tue Apr 26, 2011 9:18 pm
Location: Netherlands
Contact:

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby Aswin » Thu Jun 22, 2017 8:39 pm

Hi,

I will look into this in more detail tomorrow. For now this tip. The Chassis can also give you a Pose provider, one that works different from the one you are using. It may serve as a workaround. You get it from the Chassis using getPoseProvider.

The Pose provider you get this way uses odometry to keep track of the pose of the robot. It is optimized for use with the Chassis.
The Pose provider you are using adds up the moves the pilot makes. It is less demanding for the CPU. Also this one should work though.
My NXT blog: http://nxttime.wordpress.com/

RoboRobo
Novice
Posts: 29
Joined: Sat Jun 18, 2016 2:10 pm

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby RoboRobo » Fri Jun 23, 2017 7:23 pm

Hi all,

I have tried .getPoseProvider with Chassis and I will try to summarize what I got.
Below are my code:

Code: Select all

Wheel wheel1 = WheeledChassis.modelWheel(new EV3LargeRegulatedMotor(MotorPort.B), 4.32).offset(11.5).invert(true);
       Wheel wheel2 = WheeledChassis.modelWheel(new EV3LargeRegulatedMotor(MotorPort.D), 4.32).offset(-11.5).invert(true);
       Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 }, WheeledChassis.TYPE_DIFFERENTIAL);
      
       MovePilot pilot = new MovePilot(chassis);
      //OdometryPoseProvider pp = new OdometryPoseProvider(pilot);
            
       pilot.setLinearSpeed(20);
         pilot.setAngularSpeed(50);
         
         Navigator navigator = new Navigator(pilot);
         
         Log l = new Log();
         
         Last_Pose = chassis.getPoseProvider().getPose().toString();
         
         l.fix();
         
         navigator.goTo(20, 0, 0);
         navigator.waitForStop();
         
            
         Last_Pose = chassis.getPoseProvider().getPose().toString();
         
         l.fix();
         
         navigator.goTo(20, 20, 0);
         navigator.waitForStop();
         
      
         Last_Pose = chassis.getPoseProvider().getPose().toString();
         
         l.fix();
         


At the beginning my program was working quite well:

X:0 Y:0 H:0
X:20 Y:0 H:0
X:20 Y:20 H:90
----------------
X:0 Y:0 H:0
X:20 Y:-0 H:0
X:20 Y:20 H:0
----------------
X:0 Y:0 H:0
X:20 Y:-0 H:0
X:20 Y:20 H:-0

Then I added two more points: (20, 30, 0) and (0, 0, 0), it also worked fine:

X:0 Y:0 H:0
X:20 Y:-0 H:0
X:20 Y:20 H:0
X:20 Y:30 H:-0
X:0 Y:0 H:-0

And when I lauched my program once more, the same thing like with OdometryPoseProvider have started to happen:

:0 Y:0 H:0
X:20 Y:-0 H:0
X:20 Y:20 H:90
X:10 Y:-0 H:90
X:40 Y:-20 H:90
----------------
X:0 Y:0 H:0
X:20 Y:-0 H:0
X:0 Y:20 H:135
X:13 Y:-35 H:135
X:61 Y:-84 H:-45
----------------
X:0 Y:0 H:0
X:20 Y:-0 H:0
X:20 Y:20 H:0
X:20 Y:30 H:-0
X:-0 Y:0 H:-124

Then I came back to the simple code, the first time was ok, further trials were not so good :)

X:0 Y:0 H:0
X:20 Y:0 H:0
X:20 Y:20 H:-0
----------------
X:0 Y:0 H:0
X:20 Y:-0 H:0
X:20 Y:20 H:90
----------------
X:0 Y:0 H:0
X:20 Y:-0 H:0
X:20 Y:20 H:90
----------------
X:0 Y:0 H:0
X:20 Y:-0 H:0
X:20 Y:20 H:-0
----------------
X:0 Y:0 H:0
X:20 Y:0 H:-0
X:0 Y:20 H:135

Or did I use getPoseProvider(0 incorrectly with Chassis?

Have a nice weekend!

RoboRobo

User avatar
gloomyandy
leJOS Team Member
Posts: 5871
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby gloomyandy » Fri Jun 23, 2017 8:10 pm

It is always worth looking at the code...
https://sourceforge.net/p/lejos/ev3/cod ... gator.java

As you can see if you create the Navigator using the constructor that simply takes a pilot then the code will always create a pose provider. This means that you are going to end up with two pose providers (which may not be a good idea). You can avoid having multiple pose providers by either...
a) Getting the built in pose provider from the chassis and tell the navigator to use this provider (by using the Navigator constructor that allows you to pass in the pilot and the pose provider).
b) Use the same pose provider as used by the navigator to obtain the current pose by calling the getPoseProider that is part of the navigator class.

Both of the above methods will ensure that there is only a single pose provider tracking the robot position, in the first case it will be the one that is part of the chassis code, in the second case it will be the odometrty pose provider (but there will only be one).

But having said that it all seems rather strange that running the same program more than once gives different results. How exactly are you running the program? Are you showing us the entire program or is there any other part that could be picking up some sort of external data? Do you have any other threads running in this program? What exactly is l.fix doing? When trying to debug things like this it is almost always a good idea to create the absolute simplest test program rather then using part of a more complex one. Perhaps you could post a link to the source of a complete test program?

Can I also suggest that unless you really need your robot to end each move pointing in a particular direction you do not use the version of goto (etc) that takes three parameters. By doing so you will be requesting that the navigator jumps through extra hoops to ensure that it points in the requested direction at the end of each way point operation. This will almost certainly result in you losing the position of the robot.

I'll also repeat my request that you post a video of what is actually happening when you run these tests. Looking at the output is fine but nothing is better than actually observing what the robot actually does. At the moment I have no idea if the robot is always moving correctly and the pose you are printing is wrong, or if the pose that you are printing is correct and the robot is not moving correctly.
leJOS news https://lejosnews.wordpress.com/

RoboRobo
Novice
Posts: 29
Joined: Sat Jun 18, 2016 2:10 pm

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby RoboRobo » Sat Jun 24, 2017 7:38 am

Do you mean I should use navigator.getPoseProvider().getPose().toString() to avoid having multiple pose?

User avatar
gloomyandy
leJOS Team Member
Posts: 5871
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby gloomyandy » Sat Jun 24, 2017 8:16 am

Yes. Or as I also said, create the pose provider (or obtain it from the chassis/move pilot) and pass it into the navigator constructor. Oh and if you are going to use it multiple times just get it once and use the reference to it... Don't keep galling getPoseProvider over and over again.
leJOS news https://lejosnews.wordpress.com/

RoboRobo
Novice
Posts: 29
Joined: Sat Jun 18, 2016 2:10 pm

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby RoboRobo » Sat Jun 24, 2017 8:20 am

I created the following code:

Code: Select all

   Last_Pose = navigator.getPoseProvider().getPose().toString();
         //Last_Pose = chassis.getPoseProvider().getPose().toString();
         
         l.fix();
         
         navigator.goTo(20, 0, 0);
         navigator.waitForStop();
         
            
         Last_Pose = navigator.getPoseProvider().getPose().toString();
         
         l.fix();
         
         navigator.goTo(20, 20, 0);
         navigator.waitForStop();
         
        // LCD.clear(3);
      //LCD.drawString("P " + pp.getPose(), 0, 3);
         
         Last_Pose = navigator.getPoseProvider().getPose().toString();
         
         l.fix();


Did you mean I call it twice?

Sorry for a stupid request, but could you correct my code and show how it should look like?
Besides, l.fix() just uses BufferedWriter to create a file with robot position coordinates which I copy to this forum.

Aswin
leJOS Team Member
Posts: 310
Joined: Tue Apr 26, 2011 9:18 pm
Location: Netherlands
Contact:

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby Aswin » Sat Jun 24, 2017 12:42 pm

Hi RoboRobo,

I just ran tests on the Chassis, the MovePilot and the Navigator. In all cases I got expected output and I was unable to identify a bug. So I suspect there is something in your program that causes it to misbehave. You should debug your program yourself using the tips Andy gave you. Start with the simplest program possible and work your way up.

One other thing. leJOS navigation has several layers of functionallity, try to use the simplest layer that suits your needs (Chassis, Pilot, Navigator). There is a series of articles on navigation on leJOS News that will help you to ubnderstand the navigation stack a little bit better. Also start with conservative settings for speed and acceleration.

In regard to your other question. You need to get the PoseProvider only once. You need to get a fresh Pose from it every time you need to know the robots current location.

Below is one of the programs I used for testing. This one tested the Chassis and its PoseProvider.

Aswin

Code: Select all

package bugs;

import lejos.hardware.Brick;
import lejos.hardware.BrickFinder;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.localization.PoseProvider;
import lejos.robotics.navigation.Pose;

public class PoseProviderBug1 {
  private Brick          brick;
  private WheeledChassis chassis;
  private PoseProvider   HoloPose;

  public static void main(String[] args) {
    PoseProviderBug1 test = new PoseProviderBug1();
    for (int i = 0; i < 10; i++) {
      test.HoloPose.setPose(new Pose());
      test.dumpPose(0, 0, 0);
      test.chassis.travel(200);
      test.chassis.waitComplete();
      test.dumpPose(200, 0, 0);
      test.chassis.arc(0, 90);
      test.chassis.waitComplete();
      test.dumpPose(200, 0, 90);
      test.chassis.travel(200);
      test.chassis.waitComplete();
      test.dumpPose(200, 200, 90);
    }
  }

  PoseProviderBug1() {
    brick = BrickFinder.getLocal();
    RegulatedMotor d = new EV3LargeRegulatedMotor(brick.getPort("D"));
    RegulatedMotor a = new EV3LargeRegulatedMotor(brick.getPort("A"));
    Wheel wheel1 = WheeledChassis.modelWheel(a, 96).offset(-48).invert(true);
    Wheel wheel2 = WheeledChassis.modelWheel(d, 96).offset(48).invert(true);
    chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 }, WheeledChassis.TYPE_DIFFERENTIAL);
    HoloPose = chassis.getPoseProvider();
    chassis.setAcceleration(100, 45);
    chassis.setSpeed(100, 90);
  }

  private void dumpPose(float x, float y, float z) {
    Pose pose = HoloPose.getPose();
    System.out.println(String.format("Intended x:%3.0f, y:%4.0f, z:%4.0f  Actual x:%3.0f, y:%4.0f, z:%4.0f", x, y, z, pose.getX(),
        pose.getY(), pose.getHeading()));
  }
}

My NXT blog: http://nxttime.wordpress.com/

RoboRobo
Novice
Posts: 29
Joined: Sat Jun 18, 2016 2:10 pm

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby RoboRobo » Sat Jun 24, 2017 6:12 pm

Aswin,

thanks for your time to test this actual functionality for me. As I am new in Java, it will take some time to go your code step by step, but definitely this would be good experience!

RoboRobo

RoboRobo
Novice
Posts: 29
Joined: Sat Jun 18, 2016 2:10 pm

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby RoboRobo » Sun Jun 25, 2017 2:14 pm

Hi again,

now I am really confused... The problem seems is related to navigator itself. I wrote a simple code, you can find it below:

Code: Select all

import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;

public class Main_102 {

   public static void main(String[] args) {
      // TODO Auto-generated method stub
      Wheel wheel1 = WheeledChassis.modelWheel(new EV3LargeRegulatedMotor(MotorPort.B), 4.32).offset(11.5).invert(true);
       Wheel wheel2 = WheeledChassis.modelWheel(new EV3LargeRegulatedMotor(MotorPort.D), 4.32).offset(-11.5).invert(true);
       Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 }, WheeledChassis.TYPE_DIFFERENTIAL);
      
       MovePilot pilot = new MovePilot(chassis);
      
       pilot.setLinearSpeed(20);
         pilot.setAngularSpeed(50);
         
         Navigator navigator = new Navigator(pilot);
         
         navigator.goTo(20, 0, 0);
         navigator.goTo(20, 20, 0);
         navigator.goTo(20, 30, 0);
         navigator.goTo(0, 0, 0);
         navigator.goTo(20, 20, 0);
         navigator.waitForStop();
         
   }

}


I also add link to the video that shows robot's movements. The first run was ok, but the other three were almost randomly. I launched the program from my brick for the second and the third times, and uploaded from PC for the fourth. The brick was reloaded before launching the program. I don't know what is the reason of such movements. Maybe there are problems with my chassis settings?


https://youtu.be/Ma4wCciSlGQ

Aswin
leJOS Team Member
Posts: 310
Joined: Tue Apr 26, 2011 9:18 pm
Location: Netherlands
Contact:

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby Aswin » Sun Jun 25, 2017 3:18 pm

Hi,

Wat is the reason you go for using the navigator and not the pilot or chassis?
Can you add navigator.singleStep (true) just after you created the navigator, test your program again and share the results?

Aswin
My NXT blog: http://nxttime.wordpress.com/

User avatar
gloomyandy
leJOS Team Member
Posts: 5871
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby gloomyandy » Sun Jun 25, 2017 3:27 pm

As I have already pointed out, do not use the 3 parameter version of goto unless you really need to do so, it just confuses everything. Just use the version that takes x and y and no heading, it is much easier to see what the robot is doing if you do that. Also can I suggest that you make your robot move in a simple square and returns to the starting point. Again it is much easier to see what is going on if you do that.
leJOS news https://lejosnews.wordpress.com/

RoboRobo
Novice
Posts: 29
Joined: Sat Jun 18, 2016 2:10 pm

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby RoboRobo » Sun Jun 25, 2017 5:31 pm

Hmm, unfortunately, navigator.singleStep (true) helps not much...
Well, I started to use (x, y, heading) pose to get better understanding how navigator works, because I had the same problems with two dimensional pose definition.
Actually what I would like my robot should do, it's that a robot follows defined route. When a robot gets input from ultra sensor about obstacle, it gets current position, and then starts to do another task. After this task is completed, robot returns to last position which was got from PoseProvider, and continues on the route that was interrupted. I thought it was also nice to track current position of my robot at any moment, so I decided to use navigator. Guys, what would be your advice regarding described task, what would be the best way to proceed?

RoboRobo
Novice
Posts: 29
Joined: Sat Jun 18, 2016 2:10 pm

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby RoboRobo » Sun Jun 25, 2017 5:56 pm

I have tried square shape movement without heading positioning, also the same problems. At the second turn, robot starts to turn 135 degrees instead of 90, and after that other non-predictable turns and even different length of movements...

Aswin
leJOS Team Member
Posts: 310
Joined: Tue Apr 26, 2011 9:18 pm
Location: Netherlands
Contact:

Re: Navigator, Pilot, PoseProvider, coordinate System questions

Postby Aswin » Sun Jun 25, 2017 7:09 pm

There are several problems you are dealing with that all work together to give you these strange results.
1. You have a tracked robot. Precise steering with a tracked robot is very hard. The main reason is that there is not a fixed point at which the track touches the floor as there would be with ordinary wheels. When a tracked robot accelerates the contact point moves to the back of the track, when braking it shifts to the front. The lejos navigation stack assumes the contact point is at a fixed position to the left or right of the origin. This is your main problem.
2. The floor is not smooth. This will make your robot even less precise.
3. You assume every move is perfectly executed by the navigator and perfectly recorded by the pose provider. But these are not perfect. So you will experience a build up of small errors into a big error in the end.

You just experience the hard part of robotics, dealing with imperfect robots in a chaotic world.
My NXT blog: http://nxttime.wordpress.com/


Return to “EV3 Software”

Who is online

Users browsing this forum: No registered users and 3 guests