From 1d42c3617618c3be43cf28e4bd0f5e78d141f8e0 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Thu, 12 Nov 2020 18:34:33 +0900 Subject: [PATCH 1/4] Use tau instead of pi 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. --- .../move_group_python_interface_tutorial.py | 18 ++++++++---- doc/pick_place/src/pick_place_tutorial.cpp | 7 +++-- doc/subframes/src/subframes_tutorial.cpp | 29 ++++++++++--------- 3 files changed, 33 insertions(+), 21 deletions(-) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index aaae8f3fc..bea42adbe 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/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 `_ 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 `_, so the first + ## thing we want to do is move it to a slightly better configuration. + ## We use the constant `tau = 2*pi `_ 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 joint_goal[6] = 0 # The go command can be called with joint values, poses, or without any diff --git a/doc/pick_place/src/pick_place_tutorial.cpp b/doc/pick_place/src/pick_place_tutorial.cpp index b806e8910..65ded3df0 100644 --- a/doc/pick_place/src/pick_place_tutorial.cpp +++ b/doc/pick_place/src/pick_place_tutorial.cpp @@ -44,6 +44,9 @@ // TF2 #include +// 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); 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. */ diff --git a/doc/subframes/src/subframes_tutorial.cpp b/doc/subframes/src/subframes_tutorial.cpp index 2e7967250..9d868f5ec 100644 --- a/doc/subframes/src/subframes_tutorial.cpp +++ b/doc/subframes/src/subframes_tutorial.cpp @@ -46,6 +46,9 @@ #include #include +// 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); 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"); From 1b354382d98b866797cd606e4072af941a6dffc4 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Thu, 4 Mar 2021 00:31:52 +0900 Subject: [PATCH 2/4] Add more explicit comments --- .../scripts/move_group_python_interface_tutorial.py | 2 +- doc/pick_place/src/pick_place_tutorial.cpp | 2 +- doc/subframes/src/subframes_tutorial.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index bea42adbe..e25aec5ff 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -183,7 +183,7 @@ def go_to_joint_state(self): joint_goal[2] = 0 joint_goal[3] = -tau/4 joint_goal[4] = 0 - joint_goal[5] = tau/6 + 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 diff --git a/doc/pick_place/src/pick_place_tutorial.cpp b/doc/pick_place/src/pick_place_tutorial.cpp index 65ded3df0..a534926b5 100644 --- a/doc/pick_place/src/pick_place_tutorial.cpp +++ b/doc/pick_place/src/pick_place_tutorial.cpp @@ -155,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, tau / 4); + 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. */ diff --git a/doc/subframes/src/subframes_tutorial.cpp b/doc/subframes/src/subframes_tutorial.cpp index 9d868f5ec..3d1c4d99e 100644 --- a/doc/subframes/src/subframes_tutorial.cpp +++ b/doc/subframes/src/subframes_tutorial.cpp @@ -124,7 +124,7 @@ void spawnCollisionObjects(moveit::planning_interface::PlanningSceneInterface& p box.subframe_poses[0].position.z = 0.0 + z_offset_box; tf2::Quaternion orientation; - orientation.setRPY(tau / 4, 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 From 6ada9642628e8e3f63729bb0309765af3711bc98 Mon Sep 17 00:00:00 2001 From: Felix von Drigalski Date: Thu, 4 Mar 2021 10:02:51 +0900 Subject: [PATCH 3/4] Add clearer radian definition in MGI C++ tutorial --- .../src/move_group_interface_tutorial.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 240785060..ee873a4a0 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -45,6 +45,9 @@ #include +// 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. From c85e5901321a41c541e7195933a3b14045bf6d69 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 4 Mar 2021 11:39:30 +0100 Subject: [PATCH 4/4] fix format --- doc/move_group_interface/src/move_group_interface_tutorial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index ee873a4a0..44c62f0c3 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -181,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] = -tau/6; // -1/6 turn in 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.