1.Set up your ROS Workspace
2.Download or clone project repository into the src directory of your ROS Workspace.
3.Experiment with the forward_kinematics environment and get familiar with the robot.
4.Launch in demo mode.
5.Perform Kinematic Analysis for the robot following the project rubric.
6.Fill in the IK_server.py with your Inverse Kinematics code.
Rubric Points
1.Provide a Writeup / README that includes all the rubric points and how you addressed each one. You can submit your writeup as markdown or pdf.
This is the one
I did not use the frames set from urdf file,instead,I used the original frames set like this.
Then create the DH Table for this robot.
| Links(i) | αi-1 | ai-1 | di | θi |
|---|---|---|---|---|
| 1 | 0 | 0 | 0.75 | θ1 |
| 2 | -pi/2 | 0.35 | 0 | θ2-pi/2 |
| 3 | 0 | 1.25 | 0 | θ3 |
| 4 | -pi/2 | -0.054 | 1.5 | θ4 |
| 5 | pi/2 | 0 | 0 | θ5 |
| 6 | -pi/2 | 0 | 0 | θ6 |
| 7 | 0 | 0 | 0.303 | 0 |
Due to we are calculating the posture of end-effector,we need to add one more row to the DH Table.
After obtainning the DH Table,we can calculate the transformation matrices between each joint,the form of the matrices is like this.
Therefore,we can calculate the transformation matrix of end-effector to base in this way.
3. Decouple Inverse Kinematics problem into Inverse Position Kinematics and inverse Orientation Kinematics; doing so derive the equations to calculate all individual joint angles.
There are two solutions for θ1,because joint2 and joint3 can reach a wide range of angle.
Most of the base joint of industrial robots can only rotate about 1 round,therefore 2 solutions can satisfy all of the path.
There're two solutions for each θ1,in the equation of θ1,when k=0,robot face toward the wrist center.
k=0:
here're the solutions.
k≠0:
According to what we have got till now,we can calculate the transformation matrix of the gripper relative to frame 3,besides,we need the rotation matrix only,that'll make the work easier.
Due to rotation matrix is an element of a group called SO(3),I used transpose operation to replace the inverse operation to make the program faster(I don't see any faster though).
New that we have θ1,θ2 and θ3,we can calculate the rotation matrix of frame 6 relative to frame 3.Which is:
We can obbtain Rotation matrix of frame3 relative to base frame with this:
Given RPY,we can calculate the end-effector's orientation relative to base frame, we need a rotation matrix to transform it to be relative to frame 6,using the end-effector's rotation matrix is ok.Then, we have a target orientation,and that orientation is mapped by a rotation matrix made by R4·R5·R6,where we can extract θ4,θ5 and θ6.
Rotation matrix with θ4,θ5 and θ6:
picture:rotation_matrix_456
Which is R_3_6.Amount all the inverse trigonometric function,atan2 is the best one to retrive the angle,but,in this case,there's a little disavantage in calculation of the angle of over 180 degree,for the function is only able to return -180 degree to 180 degree.
However,there's more solutions,so we need to calculate them ourselves.Fortunately,it's easy to do that by simply plus or minus 2pi.Therefore,it would be 8 solutions for the orientation which give us a lot of choices. By combinning them to the 3 angles, we can have 32 solutions in total!
Here I just put the easiest solution.
The other solutions are like θ4 rotate 180 or 360 degree and θ5 and θ6 rotate too.
1. Fill in the IK_server.py file with properly commented python code for calculating Inverse Kinematics based on previously performed Kinematic Analysis. Your code must guide the robot to successfully complete 8/10 pick and place cycles. Briefly discuss the code you implemented and your results.
My code can guide the robot to complete 10/10 pick and place cycles,for it took me almost or more than one weeks full time to make. There's comment in the python file,so I will not introduce the process of it.I will talk about what I think is unique to others works.
1.The code will calculate 32 solutions of the robot,though not all of them are reachable.After the calculation,there will be a cost evaluation to evaluate the cost of each reachable solution,then will show the one with the lowest cost and execute it.
2.Required current joint position,the robot is able to calculate the cost of each solution,and set the current joint position by forward kinematic,and iterate till all points are calculated the IK.What matters is the cost function,I set a high cost of the movement of joint 1,the joint 2 and joint 3 have the same cost,joint 4 5 and 6 have the lowest cost,I think it's reasonable because I've learnd that next move should be close to the current gesture as much as possible.
Here's the way to run my code.First,start the safe_spawn.sh,then run the script with name of transmitter.py then run IK_server.py.
Here's some screenshots of the running code.
In the last screenshot,three curve represent the error of 3 positions of FK to the target position.