Do you know what the meaning of this code ?

asked 2018-12-22 09:13:01 -0500

Ilhamasy
18 ●6 ●8 ●8

function of fnGet2DRobotPose is to transform euler from quarternion, and the value is pos_x, pos_y, theta.

def GetRobotOdom(self, robot_odom_msg):
    if self.odom_diterima == False:
        self.odom_diterima = True 

    pos_x, pos_y, theta = self.fnGet2DRobotPose(robot_odom_msg)

    self.robot_2d_pose_x = pos_x
    self.robot_2d_pose_y = pos_y
    self.robot_2d_theta = theta

    if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.:
        d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) - 2 * math.pi
    elif (self.robot_2d_theta - self.previous_robot_2d_theta) < -5.:
        d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) + 2 * math.pi
    else:
        d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta)**

    self.total_robot_2d_theta = self.total_robot_2d_theta + d_theta
    self.previous_robot_2d_theta = self.robot_2d_theta

    self.robot_2d_theta = self.total_robot_2d_theta

please help me :(

answered 2018-12-23 06:46:01 -0500

Hamid Didari

1769 ●26 ●43 ●53

some times you want control yaw(theta) but when you transform quarternion to euler the angles are between pi and -pi and you have one break point in pi and it's bother controller for this reason in this part of code:

if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) - 2 * math.pi
elif (self.robot_2d_theta - self.previous_robot_2d_theta) < -5.:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) + 2 * math.pi
else:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta)**

if we have jump in angle (this jump is bigger than 2 *pi(-pi to pi)) Compute d_theta and add the d_theta to total_robot_2d_theta

edit flag offensive delete link

Comments

okay, thanks for your answer, but if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.: what is the meaning of >5 i still not understand that ? :(

Ilhamasy ( 2018-12-23 09:39:00 -0500 )edit

This is the problem with using magic numbers

jayess ( 2018-12-23 11:55:46 -0500 )edit

assume your teta is 3 and you have rotation your teta change like this (3,3.1,3.14,-3.14 ,-3.1,-3) you have one jump in your teta in 3.14 to -3.14 for detecting this jump you can Compute (teta-last_teta) and if the result bigger than 5 or 6 or 4 depend on your decision you have jump in your teta

Hamid Didari ( 2018-12-23 22:30:54 -0500 )edit

add a comment

Do you know what the meaning of this code ?

1 Answer

Comments

Question Tools

Stats

Related questions

Do you know what the meaning of this code ? edit

1 Answer

Comments

Question Tools

Stats

Related questions

Do you know what the meaning of this code ?