Package: rtb-toolbox
Robotic ToolsRobotic tools is a library made to make some calculations easier, like robots forward
kinematic's and dynamics. There is also an numerical implementation of inverse velocity kinematic's.You can use this lib for any robot, since you have the Denavit Hartenberg parameters.Forward Kinematicsin order to use the forward kinematics, you gonna need the robot DH parameters. Then
u can create a 'Link' object representation for each link, using the parameters.importsympyasspfromlib.linkimportLinkq1,q2,q3=sp.symbols('q_1 q_2 q_3')j0=Link([q1,450,150,sp.pi/2])j1=Link([q2,0,590,0])j2=Link([q3,0,130,sp.pi/2])Finally create an instance of the ForwardKinematic class, and pass a list with
all links in the constructor. You can also pass an offset with the angles of home position.fromlib.forward_kinematicsimportForwardKinematicfk=ForwardKinematic([j0,j1,j2],offset=np.array([.0,.0,.0]))The ForwardKinematic class contains the symbolic matrices of transformations, like transformations
from the reference frame to the i-th frame, the end-effector transformation matrix, the jacobian matrix, and other
things.Inverse KinematicsTo use the inverse kinematics u need first to have the ForwardKinematic of the robotInverse Kinematics of PositionThe inverse kinematics of position uses the Gradient Descent method to find an optimal solution
for the end-effector position.To use it, as said before, u need the ForwardKinematic. Then, just import the ik_position
method from lib.inverse_kinematics packageimportnumpyasnpfromlib.inverse_kinematicsimportik_position# PX, Py, Pzdesired_position=np.array([.1,.4,.0])thetas,_,success=ik_position(desired_position=desired_position,fk=fk,initial_guess=np.array([.2,.7,-.1]),f_tolerance=1e-5,max_iterations=1000,lmbd=.1,verbose=True)Output example of the inverse kinematics of position:Inverse Kinematics of Position and OrientationThe inverse kinematics of position and orientation uses the jacobian matrix and end-effector velocities
necessary to achive an wanted transformation. This method is also called inverse velocity kinematics. The
end-effector velocities mentioned before are calculated using the methods explained in
Modern Robotics Book (http://hades.mech.northwestern.edu/index.php/Modern_Robotics).importnumpyasnpfromlib.inverse_kinematicsimportik# Px, Py, Pz, Rx, Ry, Rzdesired_transformation=np.array([.1,.4,.0,0,np.pi/4,0])thetas,_,success=ik(desired_transformation=desired_transformation,fk=fk,initial_guess=np.array([.2,.7,-.1]),epsilon_wb=1e-5,epsilon_vb=1e-5,max_iterations=1000,lmbd=.1,verbose=True,only_position=False,normalize=False)Output example for the inverse kinematics of position and orientationForward DynamicsIn order to compute the ForwardDynamics u first need the ForwardKinematic of the robot.
When u instantiate the ForwardDynamic class, it will start to calculate the equations of motion (resulting torque's)
in each link, so it can take a long time if you use the simplify method of sympy library.The joint variables (thetas) need to be functions of time.fromlib.symbolsimporttimportsympyasspfromlib.forward_kinematicsimportForwardKinematicfromlib.forward_dynamicsimportForwardDynamicsfromlib.linkimportLink# To use the forward dynamics, the q's need to be functions of timeq1=sp.Function('q_1')(t)q2=sp.Function('q_2')(t)a1,a2=sp.symbols('a_1 a_2')j0=Link([q1,0,a1,0])j1=Link([q2,0,a2,0])rr_fk=ForwardKinematic([j0,j1])fd=ForwardDynamics(rr_fk)foreqinfd.equations:print(' ')sp.print_latex(sp.simplify(eq))print(' ')Example of forward dynamic equations of an RR planar robot