As you experiment with inverse kinematic solvers you may notice that they can be unpredictable and/or fail when executing large movements. Often it is best to further constrain motion planning by introducing intermediate points along the way to guide the arm through your desired path. This is useful for obstacle avoidance, decreasing risk of erratic movements, controlling approach movements, etc. There are many ways to interpolate poses for smooth motion planning. We will try plotting a bezier curve. A Bezier curve is a mathematical curve defined by a series of control points, which determine the shape of the curve. The curve starts at the first control point and ends at the last control point, but it can bend and twist in any way that is defined by the other control points.

During this session, we will guide the Kinova arm to follow a trajectory based on a Bezier curve, moving it from a retracted position to a vertical position. First, we will acquaint ourselves with the lab materials, starting with an Excel file employed to generate a trajectory using Bezier curves.

Excel file link

This Excel file calculates interpolate points along the Bezier curve based on 4 input control points, with two possible output versions containing 11 or 21 points.

1. Spawn a robot in an empty world:

   ros2 launch kortex_bringup kortex_sim_control.launch.py \
   robot_type:=gen3_lite \
   robot_name:=gen3_lite \
   dof:=6 \
   gripper:=gen3_lite_2f \
   launch_rviz:=false	

2. Start MoveIt:

   ros2 launch kinova_gen3_lite_moveit_config sim.launch.py

3. The start and end positions are already filled in. Use the markers and MoveIt to find 2 points between the start and end for control points of the bezier curve. Use the following command to print the position of the end-effector in the base_link frame:

   ros2 run tf2_ros tf2_echo base_link end_effector_link

4. Create a copy of the Excel file and enter your points as P1 and P2.

5. Select the version of your choice (11 or 21 points) and paste that information into a file called data.csv. Make sure you use commas for value separation instead of tabs.

6. Run the python script: plot_trajectory.py to plot the points in 3D for visualization.

   Deliverable 1.1: Attach a screenshot of your plotted trajectory.

So far, we have handled the position of the end-effector's pose along the trajectory with the Bezier curve. Next, we will need to generate smooth changes between the orientations that differ at positions.

For quaternion interpolation we will use one class of the python module pyquaternion. The typical usage of pyquaternion that best suits our application is by declaring one quaternion for each start and end pose, and use one method that uniformly interpolates values in between:

from pyquaternion import Quaternion

qStart = Quaternion(w=<value>, x=<value>, y=<value>, z=<value>)

qEnd = Quaternion(w=<value>, x=<value>, y=<value>, z=<value>)

qList = Quaternion.intermediates(qStart, qEnd, <numIntermediates>, include_endpoints=True)

This chunk of the code is in trajectory_from_csv.py. This file combines the two composition of trajectory generation - positions along the path and quaternion interpolation.

1. Create a new ros2 python package to host the code for this lab. Move trajectory_from_csv.py and gen3lite_pymoveit2.py into that package. Don't forget to include the python files in the setup.py. Move sort_world.launch.py into a subdirectory called "launch". Move sort_world.sdf into a subdirectory called "worlds". Build and source the package.

2. Run the script and visualize the changes in both RViZ and Gazebo.

   Deliverables

   1.2: Submit the selected control points used to generate a (bezier curve) trajectory to go from Home to Vertical position.

   1.3: Compare the path you created versus the path generated by Motion Planning in RViz starting from Home to Vertical. Please provide explanation when needed.

   1.4: Discuss the importance of complementing the trajectory generation process with the quaternion interpolation.

   1.5: If you have any modifications to the script that would result in a better visualization experience, propose them here and take a second screenshot.

We have now become acquainted with all the aspects related to the arm's kinematics, trajectory planning, and the use of joint angles (configuration space) or end-effector poses (task space) to direct the robot's movements. In this section, we will apply all of these elements to a sorting task, which involves picking up cubes of different colors and placing them on top of the corresponding tables.

Drawing on your prior knowledge of trajectory planning from the previous section, as well as your experience from the previous lab assignment, think about how the sorting task can be broken down into smaller sub-tasks, such as sorting the green cube and sorting the red cube. Additionally, the process for sorting the green cube can involve the same steps as the pick-and-place task from lab 6. To refresh your memory, the pick-and-place task involves the following steps:

1. Reach position above the center of the object to be picked with gripper oriented
2. Adjust gripper to open or semi-open state.
3. Decrease the end-effector's z-value position
4. Close gripper to grasp object.
5. Reach position above the desired location to place the object with gripper oriented.
6. Decrease the end-effector's z-value position
7. Open gripper to release object.
8. Increase the end-effector's z-value position to clear the object
9. Return to home position.

However, for sorting the cubes, you should consider finding waypoints that won't hit the walls, and generating trajectories that guarantee a successful movement between picking and placing.

1. Edit sort_world.launch.py and set the path to sort_world.sdf. Start the robot using the new launch file you placed in this package:

   ros2 launch <your_package> sort_world.launch.py \
   robot_type:=gen3_lite \
   robot_name:=gen3_lite \
   dof:=6 \
   gripper:=gen3_lite_2f \
   launch_rviz:=false	

2. Start MoveIt2 as you did before.

3. Reference the trajectory_from_csv.py and gen3lite_pymoveit2.py files to learn how to write a new python script to control the robot arm. Write a function to sort cubes to their respective tables. Hint: checkout the .sdf world file to see where the blocks are placed so you don't have to locate them yourself.

   Deliverables

   2.1: Provide a walkthrough of your process to complete the sorting task What sub-tasks did you select to divide the sorting task into? What did each sub-task entail? Discuss the steps within each of your sub-tasks. Mention some of the challenges and how you addressed them.

   2.2: Report values of interest used to complete the task: Poses of the cubes, the bins, the selected waypoints, and desired placing location. Selected height to approach object and place locations. Percentage of the gripper's open/close position.

   2.3: Record a video of the entire sorting task.