InverseKinematicsNumericalExample.py

You can view and download this file on Github: InverseKinematicsNumericalExample.py

 1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 2# This is an EXUDYN example
 3#
 4# Details:  example for inverse kinematics of serial manipulator UR5
 5#
 6# Author:   Peter Manzel; Johannes Gerstmayr
 7# Date:     2019-07-15
 8#
 9# Copyright:This file is part of Exudyn. Exudyn is free software. You can redistribute it and/or modify it under the terms of the Exudyn license. See 'LICENSE.txt' for more details.
10#
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13import exudyn as exu
14from exudyn.itemInterface import *
15from exudyn.utilities import *
16from exudyn.rigidBodyUtilities import *
17from exudyn.graphicsDataUtilities import *
18from exudyn.robotics import *
19import numpy as np
20
21from exudyn.kinematicTree import KinematicTree66, JointTransformMotionSubspace
22# from exudyn.robotics import InverseKinematicsNumerical
23
24jointWidth=0.1
25jointRadius=0.06
26linkWidth=0.1
27graphicsBaseList = [GraphicsDataOrthoCubePoint([0,0,-0.15], [0.12,0.12,0.1], color4grey)]
28graphicsBaseList +=[GraphicsDataCylinder([0,0,-jointWidth], [0,0,jointWidth], linkWidth*0.5, color4list[0])] #belongs to first body
29
30from exudyn.robotics.models import ManipulatorPuma560, ManipulatorPANDA, ManipulatorUR5
31# robotDef = ManipulatorPuma560()
32robotDef = ManipulatorUR5()
33# robotDef = ManipulatorPANDA()
34flagStdDH = True
35# LinkList2Robot() # todo: build robot using the utility function
36
37toolGraphics = [GraphicsDataBasis(length=0.3*0)]
38robot2 = Robot(gravity=[0,0,-9.81],
39              base = RobotBase(HT=HTtranslate([0,0,0]), visualization=VRobotBase(graphicsData=graphicsBaseList)),
40              tool = RobotTool(HT=HTtranslate([0,0,0.1*0]), visualization=VRobotTool(graphicsData=toolGraphics)),
41              referenceConfiguration = []) #referenceConfiguration created with 0s automatically
42
43nLinks = len(robotDef['links'])
44# save read DH-Parameters into variables for convenience
45a,d,alpha,rz, dx = [0]*nLinks, [0]*nLinks, [0]*nLinks, [0]*nLinks, [0]*nLinks
46for cnt, link in enumerate(robotDef['links']):
47    robot2.AddLink(RobotLink(mass=link['mass'],
48                               COM=link['COM'],
49                               inertia=link['inertia'],
50                                localHT=StdDH2HT(link['stdDH']),
51                                # localHT=StdDH2HT(link['modKKDH']),
52                               PDcontrol=(10, 1),
53                               visualization=VRobotLink(linkColor=color4list[cnt], showCOM=False, showMBSjoint=True)
54                               ))
55    # save read DH-Parameters into variables for convenience
56    if flagStdDH: # std-dh
57        # stdH = [theta, d, a, alpha] with Rz(theta) * Tz(d) * Tx(a) * Rx(alpha)
58        d[cnt], a[cnt], alpha[cnt] = link['stdDH'][1],link['stdDH'][2], link['stdDH'][3]
59    else:
60        # modDH = [alpha, dx, theta, rz] as used by Khali: Rx(alpha) * Tx(d) * Rz(theta) * Tz(r)
61        # Important note:  d(khali)=a(corke)  and r(khali)=d(corke)
62        alpha[cnt], dx[cnt], rz[cnt],  = link['stdDH'][0],link['stdDH'][1], link['stdDH'][3]
63
64myIkine = InverseKinematicsNumerical(robot2, useRenderer=True)
65## test
66if 1:  # tests close to zero-configuration
67    R = RotXYZ2RotationMatrix(np.array([np.pi,0.2*0,np.pi/8*0 ]))
68    t = [0.4526, -0.1488, 0.5275]
69    T2 = [[1,0,0,0.3], [0,1,0,0.3], [0,0,1,0.3], [0,0,0,1]]
70    T3 = HomogeneousTransformation(R, t)
71    sol = myIkine.Solve(T3, q0 = [0, -np.pi/4, -np.pi/4, -np.pi/4, np.pi/4, np.pi/2])
72    print('success = {}\nq = {} rad'.format(sol[1], np.round(sol[0], 3)))