Mpinol/simrealrebased (#107)

* Adding sim and real pnp files.

Author: mpinol

tutorials/pick_and_place/3_pick_and_place.md

@@ -68,7 +68,7 @@ Steps covered in this tutorial includes invoking a motion planning service in RO
 
     > This is similar to the `SourceDestinationPublisher.Publish()` function, but with a few key differences. There is an added `pickPoseOffset` to the `pick` and `place_pose` `y` component. This is because the calculated trajectory to grasp the `target` object will hover slightly above the object before grasping it in order to avoid potentially colliding with the object. Additionally, this function calls `CurrentJointConfig()` to assign the `request.joints_input` instead of assigning the values individually.
 
-    > The `response.trajectories` are received in the `TrajectoryResponse()` callback, as defined in the `ros.SendServiceMessage` parameters. These trajectories are passed to `ExecuteTrajectories()` below:
+    > The `response.trajectories` are received in the `TrajectoryResponse()` callback, as defined in the `ros.SendServiceMessage` parameters. These trajectories are passed to `ExecuteTrajectories()` and executed as a [coroutine](https://docs.unity3d.com/Manual/Coroutines.html):
 
     ```csharp
     private IEnumerator ExecuteTrajectories(MoverServiceResponse response)

tutorials/pick_and_place/4_pick_and_place.md

@@ -15,7 +15,8 @@ This tutorial is designed such that you do not need prior experience with Unity
   - [Part 0: ROS Setup](#part-0-ros-setup)
   - [Part 1: Create Unity scene with imported URDF](#part-1-create-unity-scene-with-imported-urdf)
   - [Part 2: ROS–Unity Integration](#part-2-rosunity-integration)
-  - [Part 3: Pick-and-Place](#part-3-pick-and-place)
+  - [Part 3: Pick-and-Place In Unity](#part-3-pick-and-place-in-unity)
+  - [Part 4: Pick-and-Place on the Real Robot](#part-4-pick-and-place-on-the-real-robot)
 
 ## Requirements
 
@@ -42,8 +43,14 @@ This part includes downloading and installing the Unity Editor, setting up a bas
 
 This part covers creating a TCP connection between Unity and ROS, generating C# scripts from a ROS msg and srv files, and publishing to a ROS topic.
 
-## [Part 3: Pick-and-Place](3_pick_and_place.md)
+## [Part 3: Pick-and-Place In Unity](3_pick_and_place.md)
 
 <img src="img/0_pick_place.gif" width="400"/>
 
-This part includes the preparation and setup necessary to run a pick-and-place task with known poses using MoveIt. Steps covered include creating and invoking a motion planning service in ROS, moving a Unity Articulation Body based on a calculated trajectory, and controlling a gripping tool to successfully grasp and drop an object.
+This part includes the preparation and setup necessary to run a pick-and-place task with known poses using MoveIt. Steps covered include creating and invoking a motion planning service in ROS, moving a Unity Articulation Body based on a calculated trajectory, and controlling a gripping tool to successfully grasp and drop an object.
+
+## [Part 4: Pick-and-Place on the Real Robot](4_pick_and_place.md)
+
+<img src="img/4_pick_and_place.gif" width="400"/>
+
+This part is going to be a little different than the previous tutorials in that it will utilize a real Niryo One robot. We do not assume that everyone has access to a Niryo One outside of simulation. As such this tutorial should mostly be used as a reference for how to move from executing commands on a simulated robot to a real one.

tutorials/pick_and_place/README.md

@@ -33,6 +33,7 @@ catkin_install_python(PROGRAMS
   scripts/mover.py
   scripts/server_endpoint.py
   scripts/trajectory_subscriber.py
+  scripts/sim_real_pnp.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

tutorials/pick_and_place/ROS/src/niryo_moveit/CMakeLists.txt

@@ -0,0 +1,5 @@
+<launch>
+    <rosparam file="$(find niryo_moveit)/config/params.yaml" command="load"/>
+    <node name="server_endpoint" pkg="niryo_moveit" type="server_endpoint.py" args="--wait" output="screen" respawn="true" />
+    <node name="sim_real_pnp" pkg="niryo_moveit" type="sim_real_pnp.py" args="--wait" output="screen"/>
+</launch>

tutorials/pick_and_place/ROS/src/niryo_moveit/launch/part_4.launch

@@ -4,8 +4,9 @@
 
 from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService
 from niryo_moveit.msg import NiryoMoveitJoints, NiryoTrajectory
-from niryo_moveit.srv import MoverService
+from niryo_moveit.srv import MoverService, MoverServiceRequest
 
+from niryo_one_msgs.msg import RobotMoveActionGoal
 
 def main():
     ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer')
@@ -16,9 +17,10 @@ def main():
     tcp_server.start({
         'SourceDestination_input': RosPublisher('SourceDestination', NiryoMoveitJoints, queue_size=10),
         'NiryoTrajectory': RosSubscriber('NiryoTrajectory', NiryoTrajectory, tcp_server),
-        'niryo_moveit': RosService('niryo_moveit', MoverService)
+        'niryo_moveit': RosService('niryo_moveit', MoverService),
+        'niryo_one/commander/robot_action/goal': RosSubscriber('niryo_one/commander/robot_action/goal', RobotMoveActionGoal, tcp_server),
+        'sim_real_pnp': RosPublisher('sim_real_pnp', MoverServiceRequest)
     })
-    
     rospy.spin()
 
 

tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py

@@ -0,0 +1,185 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+import rospy
+
+import time
+import sys
+import copy
+import math
+import moveit_commander
+import actionlib
+
+import moveit_msgs.msg
+
+from niryo_one_msgs.msg import RobotMoveAction, ToolCommand, TrajectoryPlan
+from niryo_one_msgs.msg import RobotMoveGoal
+
+from moveit_msgs.msg import Constraints, JointConstraint, PositionConstraint, OrientationConstraint, BoundingVolume
+from sensor_msgs.msg import JointState
+from moveit_msgs.msg import RobotState
+import geometry_msgs.msg
+from geometry_msgs.msg import Quaternion, Pose
+from std_msgs.msg import String
+from moveit_commander.conversions import pose_to_list
+
+from niryo_moveit.srv import MoverServiceRequest
+
+joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
+
+# Gripper parameters
+OPEN_COMMAND = 1
+CLOSE_COMMAND = 2
+GRIPPER_SPEED = 100
+
+# IDs used to determine which values in a RobotMoveGoal message to execute
+TOOL_ID = 11
+TOOL_COMMAND_ID = 6
+TRAJECTORY_COMMAND_ID = 7
+
+PICK_PLACE_HEIGHT_OFFSET = 0.065 
+
+# Between Melodic and Noetic, the return type of plan() changed. moveit_commander has no __version__ variable, so checking the python version as a proxy
+if sys.version_info >= (3, 0):
+    def planCompat(plan):
+        return plan[1]
+else:
+    def planCompat(plan):
+        return plan
+
+"""
+    Given the start angles of the robot, plan a trajectory that ends at the destination pose.
+"""
+def plan_trajectory(move_group, destination_pose, start_joint_angles): 
+    current_joint_state = JointState()
+    current_joint_state.name = joint_names
+    current_joint_state.position = start_joint_angles
+
+    moveit_robot_state = RobotState()
+    moveit_robot_state.joint_state = current_joint_state
+    move_group.set_start_state(moveit_robot_state)
+
+    move_group.set_pose_target(destination_pose)
+
+    if not move_group.plan():
+        exception_str = """
+            Trajectory could not be planned for a destination of {}\n with starting joint angles {}.
+            Please make sure target and destination are reachable by the robot.
+        """.format(destination_pose, start_joint_angles)
+        raise Exception(exception_str)
+
+    trajectory_plan = TrajectoryPlan()
+    trajectory_plan.trajectory = planCompat(move_group.plan())
+
+    return trajectory_plan
+
+
+def send_trajectory_goal(client, trajectory):
+
+    # Build the goal
+    goal = RobotMoveGoal()
+    goal.cmd.Trajectory = trajectory
+    goal.cmd.cmd_type = TRAJECTORY_COMMAND_ID
+
+    client.send_goal(goal)
+    client.wait_for_result()
+
+    return
+
+def send_tool_goal(client, gripper_command):
+    tool_command = ToolCommand()
+    tool_command.tool_id = TOOL_ID
+    tool_command.cmd_type = gripper_command
+    tool_command.gripper_open_speed = GRIPPER_SPEED
+    tool_command.gripper_close_speed = GRIPPER_SPEED
+
+    goal = RobotMoveGoal()
+    goal.cmd.tool_cmd = tool_command
+    goal.cmd.cmd_type = TOOL_COMMAND_ID
+
+    client.send_goal(goal)
+    client.wait_for_result()
+
+    return
+
+
+"""
+    Creates a pick and place plan using the four states below.
+    
+    1. Pre Grasp - position gripper directly above target object
+    2. Grasp - lower gripper so that fingers are on either side of object
+    3. Pick Up - raise gripper back to the pre grasp position
+    4. Place - move gripper to desired placement position
+
+    Gripper behaviour is handled outside of this trajectory planning.
+        - Gripper close occurs after 'grasp' position has been achieved
+        - Gripper open occurs after 'place' position has been achieved
+
+    https://github.com/ros-planning/moveit/blob/master/moveit_commander/src/moveit_commander/move_group.py
+"""
+def plan_pick_and_place(req):
+    print("Starting planning....")
+    client = actionlib.SimpleActionClient('niryo_one/commander/robot_action', RobotMoveAction)
+    client.wait_for_server()
+
+    group_name = "arm"
+    move_group = moveit_commander.MoveGroupCommander(group_name)
+
+    current_robot_joint_configuration = [
+        math.radians(req.joints_input.joint_00),
+        math.radians(req.joints_input.joint_01),
+        math.radians(req.joints_input.joint_02),
+        math.radians(req.joints_input.joint_03),
+        math.radians(req.joints_input.joint_04),
+        math.radians(req.joints_input.joint_05),
+    ]
+
+    # Pre grasp - position gripper directly above target object
+    pre_grasp_pose = plan_trajectory(move_group, req.pick_pose, current_robot_joint_configuration)
+
+    previous_ending_joint_angles = pre_grasp_pose.trajectory.joint_trajectory.points[-1].positions
+    send_trajectory_goal(client, pre_grasp_pose)
+
+    # Grasp - lower gripper so that fingers are on either side of object
+    pick_pose = copy.deepcopy(req.pick_pose)
+    pick_pose.position.z -= PICK_PLACE_HEIGHT_OFFSET  # Static value coming from Unity, TODO: pass along with request
+    grasp_pose = plan_trajectory(move_group, pick_pose, previous_ending_joint_angles)
+
+    previous_ending_joint_angles = grasp_pose.trajectory.joint_trajectory.points[-1].positions
+    send_trajectory_goal(client, grasp_pose)
+    send_tool_goal(client, CLOSE_COMMAND)
+
+    # Pick Up - raise gripper back to the pre grasp position
+    pick_up_pose = plan_trajectory(move_group, req.pick_pose, previous_ending_joint_angles)
+    previous_ending_joint_angles = pick_up_pose.trajectory.joint_trajectory.points[-1].positions
+    send_trajectory_goal(client, pick_up_pose)
+
+    # Place - move gripper to desired placement position
+    pre_place_pose = plan_trajectory(move_group, req.place_pose, previous_ending_joint_angles)
+    previous_ending_joint_angles = pre_place_pose.trajectory.joint_trajectory.points[-1].positions
+    send_trajectory_goal(client, pre_place_pose)
+
+    # Gently Place object
+    place_pose = copy.deepcopy(req.place_pose)
+    place_pose.position.z -= PICK_PLACE_HEIGHT_OFFSET  # Static value coming from Unity, TODO: pass along with request
+    place_traj = plan_trajectory(move_group, place_pose, previous_ending_joint_angles)
+    previous_ending_joint_angles = place_traj.trajectory.joint_trajectory.points[-1].positions
+    send_trajectory_goal(client, place_traj)
+    send_tool_goal(client, OPEN_COMMAND)
+
+    # Reset robot at pre place pose pose
+    post_place_traj = plan_trajectory(move_group, req.place_pose, previous_ending_joint_angles)
+    send_trajectory_goal(client, post_place_traj)
+
+    print("Finished executing action.")
+
+def listener():
+    rospy.init_node('sim_real_pnp', anonymous=True)
+
+    rospy.Subscriber("sim_real_pnp", MoverServiceRequest, plan_pick_and_place)
+
+    rospy.spin()
+
+if __name__ == '__main__':
+    listener()