Use tau instead of pi for rotations (#554)

Tau is the circle constant C/r (diameter over radius) = 2*pi = tau. This makes writing rotations easier.

See https://tauday.com/tau-manifesto for more reasoning.

* Add more explicit comments

* Add clearer radian definition in MGI C++ tutorial

Co-authored-by: v4hn <me@v4hn.de>

Author: felixvd

doc/move_group_interface/src/move_group_interface_tutorial.cpp

@@ -45,6 +45,9 @@
 
 #include <moveit_visual_tools/moveit_visual_tools.h>
 
+// The circle constant tau = 2*pi. One tau is one rotation in radians.
+const double tau = 2 * M_PI;
+
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "move_group_interface_tutorial");
@@ -178,7 +181,7 @@ int main(int argc, char** argv)
   current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
 
   // Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
-  joint_group_positions[0] = -1.0;  // radians
+  joint_group_positions[0] = -tau / 6;  // -1/6 turn in radians
   move_group.setJointValueTarget(joint_group_positions);
 
   // We lower the allowed maximum velocity and acceleration to 5% of their maximum.

doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py

@@ -52,6 +52,11 @@
 import moveit_msgs.msg
 import geometry_msgs.msg
 from math import pi, dist, fabs, cos
+try:
+  from math import tau
+except: # For Python 2 compatibility
+  from math import pi
+  tau = 2.0*pi
 from std_msgs.msg import String
 from moveit_commander.conversions import pose_to_list
 ## END_SUB_TUTORIAL
@@ -168,16 +173,17 @@ def go_to_joint_state(self):
     ##
     ## Planning to a Joint Goal
     ## ^^^^^^^^^^^^^^^^^^^^^^^^
-    ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
-    ## thing we want to do is move it to a slightly better configuration.
-    # We can get the joint values from the group and adjust some of the values:
+    ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_, so the first
+    ## thing we want to do is move it to a slightly better configuration. 
+    ## We use the constant `tau = 2*pi <https://en.wikipedia.org/wiki/Turn_(angle)#Tau_proposals>`_ for convenience:
+    # We get the joint values from the group and change some of the values:
     joint_goal = move_group.get_current_joint_values()
     joint_goal[0] = 0
-    joint_goal[1] = -pi/4
+    joint_goal[1] = -tau/8
     joint_goal[2] = 0
-    joint_goal[3] = -pi/2
+    joint_goal[3] = -tau/4
     joint_goal[4] = 0
-    joint_goal[5] = pi/3
+    joint_goal[5] = tau/6  # 1/6 of a turn
     joint_goal[6] = 0
 
     # The go command can be called with joint values, poses, or without any

doc/pick_place/src/pick_place_tutorial.cpp

@@ -44,6 +44,9 @@
 // TF2
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
+// The circle constant tau = 2*pi. One tau is one rotation in radians.
+const double tau = 2 * M_PI;
+
 void openGripper(trajectory_msgs::JointTrajectory& posture)
 {
   // BEGIN_SUB_TUTORIAL open_gripper
@@ -94,7 +97,7 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group)
   // transform from `"panda_link8"` to the palm of the end effector.
   grasps[0].grasp_pose.header.frame_id = "panda_link0";
   tf2::Quaternion orientation;
-  orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
+  orientation.setRPY(-tau / 4, -tau / 8, -tau / 4);
   grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
   grasps[0].grasp_pose.pose.position.x = 0.415;
   grasps[0].grasp_pose.pose.position.y = 0;
@@ -152,7 +155,7 @@ void place(moveit::planning_interface::MoveGroupInterface& group)
   // +++++++++++++++++++++++++++
   place_location[0].place_pose.header.frame_id = "panda_link0";
   tf2::Quaternion orientation;
-  orientation.setRPY(0, 0, M_PI / 2);
+  orientation.setRPY(0, 0, tau / 4);  // A quarter turn about the z-axis
   place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
 
   /* For place location, we set the value to the exact location of the center of the object. */

doc/subframes/src/subframes_tutorial.cpp

@@ -46,6 +46,9 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_eigen/tf2_eigen.h>
 
+// The circle constant tau = 2*pi. One tau is one rotation in radians.
+const double tau = 2 * M_PI;
+
 // BEGIN_SUB_TUTORIAL plan1
 //
 // Creating the planning request
@@ -121,35 +124,35 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p
   box.subframe_poses[0].position.z = 0.0 + z_offset_box;
 
   tf2::Quaternion orientation;
-  orientation.setRPY(90.0 / 180.0 * M_PI, 0, 0);
+  orientation.setRPY(tau / 4, 0, 0);  // A quarter turn about the x-axis
   box.subframe_poses[0].orientation = tf2::toMsg(orientation);
   // END_SUB_TUTORIAL
 
   box.subframe_names[1] = "top";
   box.subframe_poses[1].position.y = .05;
   box.subframe_poses[1].position.z = 0.0 + z_offset_box;
-  orientation.setRPY(-90.0 / 180.0 * M_PI, 0, 0);
+  orientation.setRPY(-tau / 4, 0, 0);
   box.subframe_poses[1].orientation = tf2::toMsg(orientation);
 
   box.subframe_names[2] = "corner_1";
   box.subframe_poses[2].position.x = -.025;
   box.subframe_poses[2].position.y = -.05;
   box.subframe_poses[2].position.z = -.01 + z_offset_box;
-  orientation.setRPY(90.0 / 180.0 * M_PI, 0, 0);
+  orientation.setRPY(tau / 4, 0, 0);
   box.subframe_poses[2].orientation = tf2::toMsg(orientation);
 
   box.subframe_names[3] = "corner_2";
   box.subframe_poses[3].position.x = .025;
   box.subframe_poses[3].position.y = -.05;
   box.subframe_poses[3].position.z = -.01 + z_offset_box;
-  orientation.setRPY(90.0 / 180.0 * M_PI, 0, 0);
+  orientation.setRPY(tau / 4, 0, 0);
   box.subframe_poses[3].orientation = tf2::toMsg(orientation);
 
   box.subframe_names[4] = "side";
   box.subframe_poses[4].position.x = .0;
   box.subframe_poses[4].position.y = .0;
   box.subframe_poses[4].position.z = -.01 + z_offset_box;
-  orientation.setRPY(0, 180.0 / 180.0 * M_PI, 0);
+  orientation.setRPY(0, tau / 2, 0);
   box.subframe_poses[4].orientation = tf2::toMsg(orientation);
 
   // Next, define the cylinder
@@ -165,7 +168,7 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p
   cylinder.primitive_poses[0].position.x = 0.0;
   cylinder.primitive_poses[0].position.y = 0.0;
   cylinder.primitive_poses[0].position.z = 0.0 + z_offset_cylinder;
-  orientation.setRPY(0, 90.0 / 180.0 * M_PI, 0);
+  orientation.setRPY(0, tau / 4, 0);
   cylinder.primitive_poses[0].orientation = tf2::toMsg(orientation);
 
   cylinder.subframe_poses.resize(1);
@@ -174,7 +177,7 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p
   cylinder.subframe_poses[0].position.x = 0.03;
   cylinder.subframe_poses[0].position.y = 0.0;
   cylinder.subframe_poses[0].position.z = 0.0 + z_offset_cylinder;
-  orientation.setRPY(0, 90.0 / 180.0 * M_PI, 0);
+  orientation.setRPY(0, tau / 4, 0);
   cylinder.subframe_poses[0].orientation = tf2::toMsg(orientation);
 
   // BEGIN_SUB_TUTORIAL object2
@@ -282,7 +285,7 @@ int main(int argc, char** argv)
   fixed_pose.header.frame_id = "panda_link0";
   fixed_pose.pose.position.y = -.4;
   fixed_pose.pose.position.z = .3;
-  target_orientation.setRPY(0, (-20.0 / 180.0 * M_PI), 0);
+  target_orientation.setRPY(0, (-20.0 / 360.0 * tau), 0);
   fixed_pose.pose.orientation = tf2::toMsg(target_orientation);
 
   // Set up a small command line interface to make the tutorial interactive.
@@ -316,7 +319,7 @@ int main(int argc, char** argv)
       // The target pose is given relative to a box subframe:
       target_pose.header.frame_id = "box/bottom";
       // The orientation is determined by RPY angles to align the cylinder and box subframes:
-      target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI);
+      target_orientation.setRPY(0, tau / 2, tau / 4);
       target_pose.pose.orientation = tf2::toMsg(target_orientation);
       // To keep some distance to the box, we use a small offset:
       target_pose.pose.position.z = 0.01;
@@ -330,7 +333,7 @@ int main(int argc, char** argv)
     {
       ROS_INFO_STREAM("Moving to top of box with cylinder tip");
       target_pose.header.frame_id = "box/top";
-      target_orientation.setRPY(180.0 / 180.0 * M_PI, 0, 90.0 / 180.0 * M_PI);
+      target_orientation.setRPY(tau / 2, 0, tau / 4);
       target_pose.pose.orientation = tf2::toMsg(target_orientation);
       target_pose.pose.position.z = 0.01;
       showFrames(target_pose, "cylinder/tip");
@@ -341,7 +344,7 @@ int main(int argc, char** argv)
     {
       ROS_INFO_STREAM("Moving to corner1 of box with cylinder tip");
       target_pose.header.frame_id = "box/corner_1";
-      target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI);
+      target_orientation.setRPY(0, tau / 2, tau / 4);
       target_pose.pose.orientation = tf2::toMsg(target_orientation);
       target_pose.pose.position.z = 0.01;
       showFrames(target_pose, "cylinder/tip");
@@ -350,7 +353,7 @@ int main(int argc, char** argv)
     else if (character_input == 4)
     {
       target_pose.header.frame_id = "box/corner_2";
-      target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI);
+      target_orientation.setRPY(0, tau / 2, tau / 4);
       target_pose.pose.orientation = tf2::toMsg(target_orientation);
       target_pose.pose.position.z = 0.01;
       showFrames(target_pose, "cylinder/tip");
@@ -359,7 +362,7 @@ int main(int argc, char** argv)
     else if (character_input == 5)
     {
       target_pose.header.frame_id = "box/side";
-      target_orientation.setRPY(0, 180.0 / 180.0 * M_PI, 90.0 / 180.0 * M_PI);
+      target_orientation.setRPY(0, tau / 2, tau / 4);
       target_pose.pose.orientation = tf2::toMsg(target_orientation);
       target_pose.pose.position.z = 0.01;
       showFrames(target_pose, "cylinder/tip");