This is the project of the last Modern Robotics course.
Here’s a bullet list of the algorithm’s steps (written in Matlab) used to create the trajectory and variable states of the simulations:

• Initialization:
  • Define robot dimensions and kinematics.
  • Initialize the arm joint angles.
• Define Object and End-Effector Configurations:
  • Set the initial and goal configurations of the cube.
  • Define end-effector’s standoff and grasp configurations.
• Create Robot Object: Instantiate the Robot class with the specified parameters.
• Set Controller Parameters: Define time step, total time, proportional and integral gains, and maximum speeds.
• Trajectory Generation Loop: Generate movement and manipulation trajectories.
  • For each phase: approaching, grasping, moving, releasing
• Extract Desired Trajectory and Grasp State for the feedback controller
• Feedback Control: Apply feedback control to follow the desired trajectory.
  • For each timestep:
    • Compute the current configuration of the robot.
    • Calculate the error between the current and desired configurations.
    • Update the robot’s state based on the control law.
• Data Recording:
  • Store the trajectory, state variables for importing to CoppeliaSim.
  • State variables include thirteen values: chassis phi, chassis x, chassis y, J1, J2, J3, J4, J5, W1, W2, W3, W4, gripper state; where J1 to J5 are the arm joint angles and W1 to W4 are the four wheel angles.

1. Computes the robot’s current configuration in the SE(3) matrix form.
2. Sets the control input for the robot’s motion.
3. Calculates the fixed body Jacobian for the wheeled mobile robot.
4. Calculates the body Jacobian at the robot’s current configuration.
5. Computes the full Jacobian combining the arm and base Jacobians.
6. Updates the forward kinematics of the robot, computing the end-effector’s pose.
7. Converts a trajectory stored in a cell array to a trajectory matrix.
8. Generates a trajectory for the robot’s movement and manipulator.
9. Updates the states of the entire robot (both wheeled base and arm) based on control inputs.
10. Implements feedback control to follow a trajectory with error correction.
11. Tests and adjusts the Jacobian based on joint limits to avoid collisions.