Robotics assignment

its a robotics assignment where we need to write the code for a particular robot and help it execute motion planning that is basically avoid obstacles and reach the final desired position. so my faculty has written most of the code we are supposed to fill in just a bit of it

The goal of this assignment is to implement the RRT algorithm for sampling-based motion planning. We will make use of the MoveIt! software framework to help with some needed auxiliary calls, but the motion planning algorithm will be implemented as part of the homework.

Checkout:

You must checkout assignment mp from the SVN server. Note that the name of this assignment on the server is “assignment_mp”. The assignment_mp folder will contain various packages needed for this assignment, including an assignment_mp package which is where you will be writing your own code. Do not forget to catkin_make your workspace once you check out the assignment.

For this assignment, you will also have to install some other packages:

To install MoveIt!, use the following command:
sudo apt-get install ros-noetic-moveit
Robot simulators:

For this assignment, we provide the Kuka LWR robot URDF. To start of the robot simulator, use one the following command:
roslaunch motion_planning kuka_lwr.launch
This will start a simulated robot, load the URDF to the parameter server, and start RVIZ with a proper configuration file. You will also see a familiar ring-and-arrows control, which you can drag to set the goal of the motion planner.
Starter code:

The starter code we have provided you does the following:

Reads the robot URDF from the parameter server and loads information about the joints of the robot.
Subscribes to the “/joint_states” topic in order to receive information about the current joint values for the robot (same as for the previous assignment)
Subscribes to the ‘/motion_planning_goal’ topic in order to listen for commands for the motion planner to execute.
Subscribes to the ‘/obstacle’ topic in order to listen for which obstacle is currently being published in RVIZ.
Creates a publisher for the ‘/joint_trajectory’ topic which will publish the commands for the robot to execute after motion planning.
Has a function to check if the state of the robot is valid.
Has a function which will perform inverse kinematics for you.
Your code:

You must fill out the “motion_planning” function in the starter code. This function is a callback for the /motion_planning_goal topic of the data type “geometry_msgs/Transform”. Each time the callback is invoked, you must plan and execute an appropriate joint trajectory using an RRT planner. Some more details:

The start point of motion planning is a goal expressed in joint space. The goal you receive is in end-effector space so you must perform IK on it to obtain a goal configuration in joint space. Our starter code provides you with a function that performs collision-aware IK. (Incidentally, this is done by using a MoveIt! service, but knowledge of this is not required for implementation.)
Once you have a goal in joint space, implement the RRT algorithm as discussed in class and presented on the handout. The length of the branches you add to your tree should be around 0.1.
You will need a call that verifies if a specific set of joint values produces a “valid” (collision-free) configuration. Our starter code also provides a function to do that.
Note that, for the KUKA arm, each joint range is between -PI and PI. You will need to sample random joint values inside this entire range if the planner is to succeed with the more complicated obstacles.
After planning an appropriate path to the goal, you must publish it as a trajectory. You will publish the result on the topic “/joint_trajectory” of the type “trajectory_msgs/JointTrajectory”. Use “rosmsg show JointTrajectory” to see what this message type looks like. The message you publish should be populated as follows:
the joint_names field should contain the names of your joints, in the same order as the joint values that make up your trajectory.
the points[] field should contain a list of waypoints in joint space. Each entry in this list is of the type “JointTrajectoryPoint”. In each of these entries, you must only fill in the “positions” field with the joint values at that specific waypoint. Leave all the other fields (e.g. velocities, accelerations, etc.) empty.
Note that, nominally, a trajectory should also contain timing information, as discussed in class. However, to simplify the assignment, we only ask you to fill in the position component of the trajectory.
Note that your path must always start from the current robot configuration. The starter code subscribes to the “/joint_states” topic and saves the starting configuration in the member variable self.q_current.
As discussed in class, you must “shortcut” the path returned by the RRT. Do that by directly connecting any two waypoints in your path that can be connected via a collision-free straight line segment in joint space. Here is an illustration of shortcutting:

After shortcutting, you must re-sample your trajectory in joint space. For each segment, add a number of internal samples equally spaced apart inside the segment. The number of internal samples for each segment should be the smallest possible such that no consecutive points on the path are farther than 0.5 to each other.
If you do not re-sample your trajectory, the robot will move a little bit and then immediately stop. Note that the number of internal samples you will add could be different for each segment. Here is an illustration of re-sampling:

To run the starter code:

rosrun assignment_mp mp.py
Testing your node:

When you right-click on the rings-and-arrows control and select “Move Arm” your node will receive a goal to motion plan to the current position of the control. If your node does all its work correctly and publishes a valid trajectory, the arm will move to the goal.
You can add obstacles to the scene by right-clicking the rings-and-arrows control and selecting “Obstacles”. You will have multiple choice of obstacles.
Your node must produce collision-free movement. Make sure to visually inspect your paths to make sure the robot does not go through the obstacle.
Grades:

Correct execution of paths with no obstacles: 2 points. Robot has 10 seconds to compute and execute the path.
Correct execution of paths with simple obstacle: 4 points. Robot has 60 seconds to compute and execute the path.
Correct execution of paths with hard obstacle: 4 points. Robot has 120 seconds to compute and execute the path.
All timing is computed from the time you click “Move Arm” on the marker and until the robot has completed the path.
Submission:

Remember to check the General Assignment Info and Collaboration and Late Submission Policies pages.

To submit your code you must commit it to the svn server.

To run the Autograder, navigate to the assignment_mp package and then use the commands:

bash grade_assignment_mp.sh
The robot simulator can be running when you run your autograder, but you should shut down the node which runs your mp.py.

Extra Credit:

REMEMBER: We will grade your last commit. So if you start doing the extra credit, commit and your code doesn’t work, this is what we will grade. You should make sure your final commit is a working copy.

Extra Credit 1: 1 point. Correct execution of paths with the super hard obstacle. Robot has 200 seconds to compute and execute the path.
Extra Credit 2: 2 points. Provide a solution that also works on the UR5 robot. You might need to provide additional packages and/or files to make this happen – figure it out! The online MoveIt! documentation is a good place to start. The same mp.py script must work on both robots. You must fill out a launch file for the UR5 to be used instead of kuka_lwr.launch. This launch file can be found in the motion planning package (ur5.launch). The TAs and the professor will not provide support for this extra credit.
Our code assumes that the UR5 configuration only has one MoveIt! group. You should make sure that your MoveIt! config package for the UR5 complies with this assumption.
We note that the UR5 end effector will point a different way along the control marker than the Kuka. This is normal and expected.
To see if your extra credit is working, we provide you with another autograder. To run it, navigate to the assignment_mp package and run:
bash grade_assignment_mp_ur5.sh
If you are implementing Extra Credit 2 (UR5), you must also submit any additional packages that you must create along the way.
To add packages to your svn repo
svn add NAME_OF_PACKAGE_OR_FILE
After you have added packages, when you type “svn status”, none of the files you want to add to svn should have a question mark by them.
Once the packages have been added, you can commit as usual. We highly recommend committing your working version of the basic assignment before you commit extra credit. Once you have svn committed and svn uped, there should be no files listed when you type into terminal “svn status”.