If u wanna find the joint angle here what can i explain, using task space. If u really wanna using joint space i will explain later.
What u asked for is Resolved Motion Rate Control, here is the step :
Note that im using Euler Angle as representation of end-effector orientation, thus matrix x will contain (in my case for 6 DOF robot):
while matrix q
xinit = Current end-effector positionpose
xfinal = end effector position-effector pose goal
in order to obtain end-effector pose, just read the book. If i explain here i will rewrite a book. And because im using euler angle, you need to use analytical jacobian.
- Trajectory Planning
Make the manipulator move smoothly
xref = xinit + (xfinal-xinit) * CurrentStep/TotalStep
- Find derivative
- PD Controller
Control the speed
- Inverse Jacobian
Find q acceleration
- Double Integral
Now we need q for current state
- Memory
And repeat this step until Current step reach total step, hope this help
For translational this step should work(just use 3 upper line of jacobian). And if u wanna control the rotational, u could use several method such as Two Vector Representation or u can use Analytical Jacobian. read my post here
The difference between task space and joint space is in task space we use controller to control end effector position (x,y,z,phi,theta,psi). While in joint space we use controller to control every single joint to reach the goal obtained from inverse jacobian.
Note : if you use euler angle as representation, you could get error because gimbal lock. i didnt how to deal with it until this time.
Reference :
- Craig, John. J. (2005). Introduction to Robotic : Mechanics and Control. Pearson Education Inc.
- Corke, Peter P. (2017). Robotics, Vision and Control. Springer.
- Siciliano, B. (2009). Robotic Modelling, Planning and Contol. Springer.
- Paul, P.R. (1981). Robot Manipulator : Mathematics, Programming, and Control. MIT.