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 * #includes itemInterface and rigidBodyUtilities
 17import exudyn.graphics as graphics
 18from exudyn.rigidBodyUtilities 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 = [graphics.Brick([0,0,-0.35], [0.12,0.12,0.5], graphics.color.grey)]
 52graphicsBaseList +=[graphics.Cylinder([0,0,0], [0.5,0,0], 0.0025, graphics.color.red)]
 53graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0.5,0], 0.0025, graphics.color.green)]
 54graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0,0.5], 0.0025, graphics.color.blue)]
 55graphicsBaseList +=[graphics.Cylinder([0,0,-jointWidth], [0,0,jointWidth], linkWidth*0.5, graphics.colorList[0])] #belongs to first body
 56graphicsBaseList +=[graphics.CheckerBoard([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 = [graphics.Cylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=graphics.color.red)]
 63graphicsToolList+= [graphics.Brick([0,ty,1.5*tz+zOff], toolSize, graphics.color.grey)]
 64graphicsToolList+= [graphics.Brick([0,-ty,1.5*tz+zOff], toolSize, graphics.color.grey)]
 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 = exu.HT(translation=[0,0,0.08])
 72
 73robot = Robot(gravity=[0,0,-9.81],
 74              base = RobotBase(HT=exu.HT(), 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.renderer.Attach()
 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 = exu.HT(rotation=RotationMatrixX(-0.3*pi), translation=[-0.45,0.,0.]) #goes through singularity
111elif motionCase == 2: 
112    HTmove = exu.HT(rotation=RotationMatrixX(0.3*pi), translation=[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 = [graphics.Frame(HTlastJoint, length=0.3, colors=[graphics.color.lightgrey]*3)]
147gFrame += [graphics.Frame(HTlastJoint@HTmove, length=0.3, colors=[graphics.color.grey]*3)]
148gFrame += [graphics.Frame(HTlastJoint@HTmove@HTmove, length=0.3, colors=[graphics.color.dodgerblue]*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.view0.window.renderWindowSize=[1920,1200]
211SC.visualizationSettings.general.graphicsUpdateInterval = 0.01
212SC.visualizationSettings.openGL.light0.shadow=0.3
213SC.visualizationSettings.view0.camera.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.solution.file.writePeriod = 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.solution.sensors.writePeriod = 0.005
237#simulationSettings.solution.file.write = False
238# simulationSettings.timeIntegration.realtime.active = True
239# simulationSettings.timeIntegration.realtime.factor = 0.025 # slow down simulation to look at
240
241simulationSettings.timeIntegration.verboseMode = 1
242simulationSettings.show.computationTime = True
243# simulationSettings.show.statistics = True
244#simulationSettings.linearSolver.solverType = 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    SC.renderer.Start()
259    SC.renderer.RestoreSavedState()
260    SC.renderer.DoIdleTasks()
261    
262# hte the simulation with the set up simulationsettings is started 
263mbs.SolveDynamic(simulationSettings, 
264                 #solverType = exudyn.DynamicSolverType.TrapezoidalIndex2, # different solver types can be used depending on the problem
265                 showHints=True)
266
267
268if useGraphics: # when graphics are deactivated the renderer does not need to be stopped
269    SC.visualizationSettings.general.autoFitScene = False
270    SC.renderer.Stop()
271
272#%%++++++++++++++++++++++++++++++++++++++++++++
273if True:
274    # import the solution viewer which loads 
275    
276    mbs.SolutionViewer()
277#%%++++++++++++++++++++++++++++++++++++++++++++
278
279
280q = mbs.GetObjectOutputBody(oKT, exu.OutputVariableType.Coordinates)
281exu.Print("rotations at tEnd=", np.sum(q), ',', q)
282    
283#%%++++++++++++++++++++++++++++++++++++++++++++
284if True:
285    
286    labels = []
287    for i in range(6):
288        labels += ['joint '+str(i)]
289    # plot data from the sensor sJoints 6 times and take the values 0 to 5 in the steps
290    mbs.PlotSensor(sensorNumbers=[sJoints]*6, components=list(np.arange(6)), yLabel='joint coordinates', labels=labels)