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)