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