ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Wed, 05 Aug 2020 04:51:41 -0500differential drive odometry equations in pythonhttps://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/I am working on a ros 2wd robot project. I am using ros kinetic.
When i calculate the odom y axis will get huge error.
For example i drive the car go forward 10 meters.Then get back the initial position (without any turn theta=0 ). x and y will ok. But if i turn 180 degress and get back to initial position , calculation of position shows me x=0 y= -4 meters. Y error is huge i think. There should be a mistake in my code.
#dt cycle time
#vty theta
self.vx= (self.vxr+self.vxl)/2.0 #vx velocity
self.vth= (self.vxr-self.vxl)/(0.6) #theta
self.dth = self.vth * dt #angular distance in 1 cycle
self.pth += self.dth #angular last position
self.vxx= self.vx * math.cos(self.vth) #velocity in x axis
self.vxy= (-1*self.vx * math.sin(self.vth)) #velocity in y axis
self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt #distance in x axis in 1 cycle
self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt #distance in y axis in 1 cycle
self.px += self.dx #last position in x axis
self.py += self.dy #last position in y axisTue, 04 Aug 2020 04:00:23 -0500https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/Comment by Weasfas for <p>I am working on a ros 2wd robot project. I am using ros kinetic.</p>
<p>When i calculate the odom y axis will get huge error.
For example i drive the car go forward 10 meters.Then get back the initial position (without any turn theta=0 ). x and y will ok. But if i turn 180 degress and get back to initial position , calculation of position shows me x=0 y= -4 meters. Y error is huge i think. There should be a mistake in my code. </p>
<pre><code>#dt cycle time
#vty theta
self.vx= (self.vxr+self.vxl)/2.0 #vx velocity
self.vth= (self.vxr-self.vxl)/(0.6) #theta
self.dth = self.vth * dt #angular distance in 1 cycle
self.pth += self.dth #angular last position
self.vxx= self.vx * math.cos(self.vth) #velocity in x axis
self.vxy= (-1*self.vx * math.sin(self.vth)) #velocity in y axis
self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt #distance in x axis in 1 cycle
self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt #distance in y axis in 1 cycle
self.px += self.dx #last position in x axis
self.py += self.dy #last position in y axis
</code></pre>
https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358713#post-id-358713You need the time from the beginning of computation if not, how you are supposed to accumulate odometry values? You really need to know the time passed since you started the initial movement. That is the reason why `last_time` is the time at which the first robot movement started. You are already calculating the cycle with the `current_time`. As for the orientation you should use the `pth` because is the accumulated value and you want to reflect in your odometry how much the robot moved from the initial position, same with the `x` and `y`, if you do not accumulate the values like that, the final result will not be real odometry values since they would not be calculated as an accumulation of the origin and they really cannot tell you where you are globally and how much the robot moved.Wed, 05 Aug 2020 04:51:41 -0500https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358713#post-id-358713Comment by bfdmetu for <p>I am working on a ros 2wd robot project. I am using ros kinetic.</p>
<p>When i calculate the odom y axis will get huge error.
For example i drive the car go forward 10 meters.Then get back the initial position (without any turn theta=0 ). x and y will ok. But if i turn 180 degress and get back to initial position , calculation of position shows me x=0 y= -4 meters. Y error is huge i think. There should be a mistake in my code. </p>
<pre><code>#dt cycle time
#vty theta
self.vx= (self.vxr+self.vxl)/2.0 #vx velocity
self.vth= (self.vxr-self.vxl)/(0.6) #theta
self.dth = self.vth * dt #angular distance in 1 cycle
self.pth += self.dth #angular last position
self.vxx= self.vx * math.cos(self.vth) #velocity in x axis
self.vxy= (-1*self.vx * math.sin(self.vth)) #velocity in y axis
self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt #distance in x axis in 1 cycle
self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt #distance in y axis in 1 cycle
self.px += self.dx #last position in x axis
self.py += self.dy #last position in y axis
</code></pre>
https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358698#post-id-358698current_time = rospy.Time.now()
dt = (current_time - self.last_time).to_sec()
self.last_time = rospy.Time.now()
In my code dt is equals only one cycle time. Do i need one cycle time or total time from begining. And i update self.last_time after calculations of dt every cycle. In that way i try to calculate one cycle time...
In addition i compute th (in my code pth - last angular position ) before the compute dx and dy. Because i use pth in equations of dx and dy. dth is angular distance in one cycle . if i turn my robot 180 degrees and go forward , will i use dth or pth ? If i understand right way, you suggest me that i should use dth (only 1 cycle angular difference)Wed, 05 Aug 2020 02:23:18 -0500https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358698#post-id-358698Comment by Weasfas for <p>I am working on a ros 2wd robot project. I am using ros kinetic.</p>
<p>When i calculate the odom y axis will get huge error.
For example i drive the car go forward 10 meters.Then get back the initial position (without any turn theta=0 ). x and y will ok. But if i turn 180 degress and get back to initial position , calculation of position shows me x=0 y= -4 meters. Y error is huge i think. There should be a mistake in my code. </p>
<pre><code>#dt cycle time
#vty theta
self.vx= (self.vxr+self.vxl)/2.0 #vx velocity
self.vth= (self.vxr-self.vxl)/(0.6) #theta
self.dth = self.vth * dt #angular distance in 1 cycle
self.pth += self.dth #angular last position
self.vxx= self.vx * math.cos(self.vth) #velocity in x axis
self.vxy= (-1*self.vx * math.sin(self.vth)) #velocity in y axis
self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt #distance in x axis in 1 cycle
self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt #distance in y axis in 1 cycle
self.px += self.dx #last position in x axis
self.py += self.dy #last position in y axis
</code></pre>
https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358649#post-id-358649@bfdmetu, you should be using `dt` that is the time difference between the beginning of time and the current computed time, since you need to keep track of the time passed since the beginning of computations. In fact in that part you need to compute `x`,`y` and `th` not only `x` and `y`. And then do the accumulation of the three variables, that is:
dt = (current_time-last_time) (in secs)
self.dx = ...
self.dy = ...
self.dth = ...
self.px += self.dx
self.py += self.dy
self.pth += self.dth
If you notice you are computing `dx` and `dy` with the accoumulated `th` from the last cycle not the current one.Tue, 04 Aug 2020 06:01:17 -0500https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358649#post-id-358649Comment by bfdmetu for <p>I am working on a ros 2wd robot project. I am using ros kinetic.</p>
<p>When i calculate the odom y axis will get huge error.
For example i drive the car go forward 10 meters.Then get back the initial position (without any turn theta=0 ). x and y will ok. But if i turn 180 degress and get back to initial position , calculation of position shows me x=0 y= -4 meters. Y error is huge i think. There should be a mistake in my code. </p>
<pre><code>#dt cycle time
#vty theta
self.vx= (self.vxr+self.vxl)/2.0 #vx velocity
self.vth= (self.vxr-self.vxl)/(0.6) #theta
self.dth = self.vth * dt #angular distance in 1 cycle
self.pth += self.dth #angular last position
self.vxx= self.vx * math.cos(self.vth) #velocity in x axis
self.vxy= (-1*self.vx * math.sin(self.vth)) #velocity in y axis
self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt #distance in x axis in 1 cycle
self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt #distance in y axis in 1 cycle
self.px += self.dx #last position in x axis
self.py += self.dy #last position in y axis
</code></pre>
https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358646#post-id-358646First of all thanks for your response. i confused some parts of the code. in the last point of the code (calculating dx and dy ) should i use dth ( angular distance in 1 cycle ) or pth ( angular last position)
and also i am not sure are dx and dy equations true or false ?Tue, 04 Aug 2020 05:48:57 -0500https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358646#post-id-358646Comment by Weasfas for <p>I am working on a ros 2wd robot project. I am using ros kinetic.</p>
<p>When i calculate the odom y axis will get huge error.
For example i drive the car go forward 10 meters.Then get back the initial position (without any turn theta=0 ). x and y will ok. But if i turn 180 degress and get back to initial position , calculation of position shows me x=0 y= -4 meters. Y error is huge i think. There should be a mistake in my code. </p>
<pre><code>#dt cycle time
#vty theta
self.vx= (self.vxr+self.vxl)/2.0 #vx velocity
self.vth= (self.vxr-self.vxl)/(0.6) #theta
self.dth = self.vth * dt #angular distance in 1 cycle
self.pth += self.dth #angular last position
self.vxx= self.vx * math.cos(self.vth) #velocity in x axis
self.vxy= (-1*self.vx * math.sin(self.vth)) #velocity in y axis
self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt #distance in x axis in 1 cycle
self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt #distance in y axis in 1 cycle
self.px += self.dx #last position in x axis
self.py += self.dy #last position in y axis
</code></pre>
https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358643#post-id-358643@bfdmetu, Technically there is no problem in that since it is the natural behaviour of every odometry computed from encoders. Since the robot does not have a perfect movement: maybe the surface is slipery or whatever other physical or mechanical factor; you always end up accumulating the errors computed from wheels. That's one of the reason why SLAM with only wheel odometry is pretty inaccurate.
You are doing something very similar to the approach the navigation tutorials explains [here](http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom). And as I said it is not something related with the code but with the natural behaviour of odometry calculations. Nervertheless I will recommend you not only basiing your localization in wheel odometry but try different approaches and odometry fusion like [robot_localization](http://wiki.ros.org/robot_localization). Or if you have sensor probabilistic localization like [AMCL](http://wiki.ros.org/amcl).Tue, 04 Aug 2020 05:24:28 -0500https://answers.ros.org/question/358633/differential-drive-odometry-equations-in-python/?comment=358643#post-id-358643