serialRobotInverseKinematics.py
You can view and download this file on Github: serialRobotInverseKinematics.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Example of a serial robot with minimal coordinates using the inverse kinematics Funcion of Exudyn
5#
6# Author: Johannes Gerstmayr
7# Date: 2023-03-29
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#q
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13
14import exudyn as exu
15from exudyn.itemInterface import *
16from exudyn.utilities import *
17from exudyn.rigidBodyUtilities import *
18from exudyn.graphicsDataUtilities import *
19from exudyn.robotics import *
20from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP
21from exudyn.robotics.models import ManipulatorPuma560, ManipulatorPANDA, ManipulatorUR5, LinkDict2Robot
22from exudyn.lieGroupBasics import LogSE3, ExpSE3
23
24import numpy as np
25
26exu.RequireVersion('1.6.31')
27# exuVersion = np.array(exu.__version__.split('.')[:3], dtype=int)
28exuVersion = exu.__version__.split('.')[:3]
29exuVersion = exuVersion[0] + exuVersion[1] + exuVersion[2]
30if int(exuVersion) < 1631:
31 print('\nWarning: use at least exudyn verion 1.6.31.dev1')
32 print('You can install the latest development version with\npip install -U exudyn --pre\n\n')
33
34
35#%% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
36#motion planning:
37jointSpaceInterpolation = False #false interpolates TCP position in work space/Cartesian coordinates
38motionCase = 2 # case 1 and 2 move in different planes
39
40
41#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
42#kinematic tree and redundant mbs agrees for stdDH version up to 1e-10, with compensateStaticTorques = False
43# KT: rotations at tEnd= 1.8464514676503092 , [0.4921990591981066, 0.2718999073958087, 0.818158053005264, -0.0030588904101585936, 0.26831938569719394, -0.0010660472359057434]
44# red. MBS:rotations at tEnd= 1.8464514674961 , [ 0.49219906 0.27189991 0.81815805 -0.00305889 0.26831939 -0.00106605]
45
46#some graphics settings to get base and tool visualization
47jointWidth=0.1
48jointRadius=0.06
49linkWidth=0.1
50
51graphicsBaseList = [GraphicsDataOrthoCubePoint([0,0,-0.35], [0.12,0.12,0.5], color4grey)]
52graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0.5,0,0], 0.0025, color4red)]
53graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0.5,0], 0.0025, color4green)]
54graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0,0.5], 0.0025, color4blue)]
55graphicsBaseList +=[GraphicsDataCylinder([0,0,-jointWidth], [0,0,jointWidth], linkWidth*0.5, color4list[0])] #belongs to first body
56graphicsBaseList +=[GraphicsDataCheckerBoard([0,0,-0.6], [0,0,1], size=2)]
57
58ty = 0.03
59tz = 0.04
60zOff = -0.05
61toolSize= [0.05,0.5*ty,0.06]
62graphicsToolList = [GraphicsDataCylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=color4red)]
63graphicsToolList+= [GraphicsDataOrthoCubePoint([0,ty,1.5*tz+zOff], toolSize, color4grey)]
64graphicsToolList+= [GraphicsDataOrthoCubePoint([0,-ty,1.5*tz+zOff], toolSize, color4grey)]
65
66# here another robot could be loaded; note that the robots sizes and workspaces and zero configuration
67# differ, so the HTmove may need to be adjusted to be inside the workspace
68robotDef = ManipulatorPuma560()
69# robotDef = ManipulatorUR5()
70# robotDef = ManipulatorPANDA()
71HTtool = HTtranslate([0,0,0.08])
72
73robot = Robot(gravity=[0,0,-9.81],
74 base = RobotBase(HT=HTtranslate([0,0,0]), visualization=VRobotBase(graphicsData=graphicsBaseList)),
75 tool = RobotTool(HT=HTtool, visualization=VRobotTool(graphicsData=graphicsToolList)),
76 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
77
78robot=LinkDict2Robot(robotDef, robot)
79
80ik = InverseKinematicsNumerical(robot=robot, useRenderer=False,
81 #flagDebug=True,
82 #jointStiffness=1e4
83 ) #, useAlternativeConstraints=True)
84SC = exu.SystemContainer()
85SC.AttachToRenderEngine()
86mbs = SC.AddSystem()
87# the system container holds one or several systems; the inverse kinematics uses a second system container without visualization
88# in the mbs the mainsystem (multibody system) is stored; in the SC more than one mbs can be created but they do not interact with each other.
89
90
91#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
92#configurations and trajectory
93q0 = [0,0,0,0,0,0] #zero angle configuration
94# q1 = [0, pi/8, pi*0.5, 0,pi/8,0] #configuration 1
95# q2 = [0.8*pi,-0.8*pi, -pi*0.5,0.75*pi,-pi*0.4,pi*0.4] #configuration 2
96# q3 = [0.5*pi,0,-0.25*pi,0,0,0] # configuration 3
97
98
99#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
100#create robot model in mbs
101#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
102
103mbs.variables['myIkine'] = ik
104jointHTs = robot.JointHT(q0)
105HTlastJoint = jointHTs[-1]@HTtool
106
107#prescribed motion:
108#HTmove = HTtranslate([-0.25,0.,0.3])
109if motionCase == 1:
110 HTmove = HT(RotationMatrixX(-0.3*pi),[-0.45,0.,0.]) #goes through singularity
111elif motionCase == 2:
112 HTmove = HT(RotationMatrixX(0.3*pi),[0.,0.,-0.3]) #no singularity
113else:
114 print('no valid motionCase provided')
115
116## here another motionCase/HTmove could be added
117
118logMove = LogSE3(HTmove)
119rotMove = Skew2Vec(logMove[0:3,0:3])
120dispMove = HT2translation(logMove)
121
122q0=[0,0,0,0.01,0.02,0.03]
123[q1, success] = ik.SolveSafe(HTlastJoint@HTmove, q0)
124if not success: print('[q1, success]=',[q1, success])
125[q2, success] = ik.SolveSafe(HTlastJoint@HTmove@HTmove, q1)
126if not success: print('[q2, success]=',[q2, success])
127
128#initialize for first simulation step:
129[q, success] = ik.Solve(HTlastJoint, q0)
130print('[q, success]=',[q, success])
131
132#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
133#trajectory generated with optimal acceleration profiles, for joint-interpolation:
134trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
135trajectory.Add(ProfileConstantAcceleration(q1,1))
136trajectory.Add(ProfileConstantAcceleration(q1,1))
137trajectory.Add(ProfileConstantAcceleration(q2,1))
138trajectory.Add(ProfileConstantAcceleration(q2,1))
139#traj.Add(ProfilePTP([1,1],syncAccTimes=False, maxVelocities=[1,1], maxAccelerations=[5,5]))
140#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
141
142
143
144
145#use frame for prescribed TCP:
146gFrame = [GraphicsDataFrame(HTlastJoint, length=0.3, colors=[color4lightgrey]*3)]
147gFrame += [GraphicsDataFrame(HTlastJoint@HTmove, length=0.3, colors=[color4grey]*3)]
148gFrame += [GraphicsDataFrame(HTlastJoint@HTmove@HTmove, length=0.3, colors=[color4dodgerblue]*3)]
149oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=gFrame)))
150
151robotDict = robot.CreateKinematicTree(mbs)
152oKT = robotDict['objectKinematicTree']
153
154#add sensor for joint coordinates (relative to reference coordinates)
155sJoints = mbs.AddSensor(SensorNode(nodeNumber=robotDict['nodeGeneric'], storeInternal=True,
156 outputVariableType=exu.OutputVariableType.Coordinates))
157
158sPosTCP = mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=5, localPosition=HT2translation(HTtool),
159 storeInternal=True,
160 outputVariableType=exu.OutputVariableType.Position))
161
162sRotTCP = mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=5, localPosition=HT2translation(HTtool),
163 storeInternal=True,
164 outputVariableType=exu.OutputVariableType.RotationMatrix))
165
166#user function which is called only once per step, speeds up simulation drastically
167def PreStepUF(mbs, t):
168 if not jointSpaceInterpolation:
169 ik = mbs.variables['myIkine']
170 if t < 2:
171 tt = min(t,1)
172 T = HTlastJoint@ExpSE3(list(tt*dispMove)+list(tt*rotMove))
173 elif t < 4:
174 tt=min(t-2,1)
175 T = HTlastJoint@HTmove@ExpSE3(list(tt*dispMove)+list(tt*rotMove))
176 else:
177 T = HTlastJoint@HTmove@HTmove
178
179 [q,success]=ik.Solve(T) #, q0) #takes 60 us internally
180 mbs.SetObjectParameter(oKT, 'jointPositionOffsetVector', q)
181
182 else:
183 #standard trajectory planning:
184
185 [u,v,a] = trajectory.Evaluate(t)
186 #in case of kinematic tree, very simple operations!
187 mbs.SetObjectParameter(oKT, 'jointPositionOffsetVector', u)
188 mbs.SetObjectParameter(oKT, 'jointVelocityOffsetVector', v)
189
190 return True
191
192mbs.SetPreStepUserFunction(PreStepUF)
193
194
195
196
197mbs.Assemble() # assemble the dynamic system
198#mbs.systemData.Info()
199
200#%% setting for visualization
201SC.visualizationSettings.connectors.showJointAxes = True
202SC.visualizationSettings.connectors.jointAxesLength = 0.02
203SC.visualizationSettings.connectors.jointAxesRadius = 0.002
204SC.visualizationSettings.nodes.showBasis = True
205SC.visualizationSettings.nodes.basisSize = 0.1
206SC.visualizationSettings.loads.show = False # shows external loads
207SC.visualizationSettings.bodies.kinematicTree.showJointFrames = False # shows the frames for each joint of the robot
208SC.visualizationSettings.openGL.multiSampling=4
209SC.visualizationSettings.general.autoFitScene=False
210SC.visualizationSettings.window.renderWindowSize=[1920,1200]
211SC.visualizationSettings.general.graphicsUpdateInterval = 0.01
212SC.visualizationSettings.openGL.shadow=0.3
213SC.visualizationSettings.openGL.perspective=0.5
214
215#traces:
216SC.visualizationSettings.sensors.traces.listOfPositionSensors = [sPosTCP]
217SC.visualizationSettings.sensors.traces.listOfTriadSensors =[sRotTCP]
218SC.visualizationSettings.sensors.traces.showPositionTrace=True
219SC.visualizationSettings.sensors.traces.showTriads=True
220SC.visualizationSettings.sensors.traces.showVectors=False
221SC.visualizationSettings.sensors.traces.showFuture=False
222SC.visualizationSettings.sensors.traces.triadsShowEvery=5
223
224#%%
225tEnd = 5 # endtime of the simulation
226h = 0.002 #500 steps take 0.16 seconds, 0.3ms / step (83% Python + inverse kinematics)
227
228# settings for the time integration / simulation
229simulationSettings = exu.SimulationSettings() #takes currently set values or default values
230simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
231simulationSettings.timeIntegration.endTime = tEnd
232simulationSettings.solutionSettings.solutionWritePeriod = 0.02
233# determines the timesteps in which the solution is saved; when it is decreased more solutions are saved, which also
234# enables the solutionViewer to
235
236simulationSettings.solutionSettings.sensorsWritePeriod = 0.005
237#simulationSettings.solutionSettings.writeSolutionToFile = False
238# simulationSettings.timeIntegration.simulateInRealtime = True
239# simulationSettings.timeIntegration.realtimeFactor = 0.025 # slow down simulation to look at
240
241simulationSettings.timeIntegration.verboseMode = 1
242simulationSettings.displayComputationTime = True
243# simulationSettings.displayStatistics = True
244#simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
245simulationSettings.timeIntegration.newton.useModifiedNewton = True
246
247# solver type
248simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
249
250#%%
251useGraphics = True
252# if useGraphics is false rendering while calculating the solution is dectivated,
253# the solution is still saved and can be then visualized afterwards for the solution viewer
254
255
256if useGraphics:
257 # start graphics
258 exu.StartRenderer()
259 if 'renderState' in exu.sys:
260 SC.SetRenderState(exu.sys['renderState'])
261 mbs.WaitForUserToContinue()
262
263# hte the simulation with the set up simulationsettings is started
264mbs.SolveDynamic(simulationSettings,
265 #solverType = exudyn.DynamicSolverType.TrapezoidalIndex2, # different solver types can be used depending on the problem
266 showHints=True)
267
268
269if useGraphics: # when graphics are deactivated the renderer does not need to be stopped
270 SC.visualizationSettings.general.autoFitScene = False
271 exu.StopRenderer()
272
273#%%++++++++++++++++++++++++++++++++++++++++++++
274if True:
275 # import the solution viewer which loads
276
277 mbs.SolutionViewer()
278#%%++++++++++++++++++++++++++++++++++++++++++++
279
280
281q = mbs.GetObjectOutput(oKT, exu.OutputVariableType.Coordinates)
282exu.Print("rotations at tEnd=", VSum(q), ',', q)
283
284#%%++++++++++++++++++++++++++++++++++++++++++++
285if True:
286
287 labels = []
288 for i in range(6):
289 labels += ['joint '+str(i)]
290 # plot data from the sensor sJoints 6 times and take the values 0 to 5 in the steps
291 mbs.PlotSensor(sensorNumbers=[sJoints]*6, components=list(np.arange(6)), yLabel='joint coordinates', labels=labels)