.. _examples-serialrobotinversekinematics: ******************************* serialRobotInverseKinematics.py ******************************* You can view and download this file on Github: `serialRobotInverseKinematics.py `_ .. code-block:: python :linenos: #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # This is an EXUDYN example # # Details: Example of a serial robot with minimal coordinates using the inverse kinematics Funcion of Exudyn # # Author: Johannes Gerstmayr # Date: 2023-03-29 # # 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. #q #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 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 * from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP from exudyn.robotics.models import ManipulatorPuma560, ManipulatorPANDA, ManipulatorUR5, LinkDict2Robot from exudyn.lieGroupBasics import LogSE3, ExpSE3 import numpy as np exu.RequireVersion('1.6.31') # exuVersion = np.array(exu.__version__.split('.')[:3], dtype=int) exuVersion = exu.__version__.split('.')[:3] exuVersion = exuVersion[0] + exuVersion[1] + exuVersion[2] if int(exuVersion) < 1631: print('\nWarning: use at least exudyn verion 1.6.31.dev1') print('You can install the latest development version with\npip install -U exudyn --pre\n\n') #%% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #motion planning: jointSpaceInterpolation = False #false interpolates TCP position in work space/Cartesian coordinates motionCase = 2 # case 1 and 2 move in different planes #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #kinematic tree and redundant mbs agrees for stdDH version up to 1e-10, with compensateStaticTorques = False # KT: rotations at tEnd= 1.8464514676503092 , [0.4921990591981066, 0.2718999073958087, 0.818158053005264, -0.0030588904101585936, 0.26831938569719394, -0.0010660472359057434] # red. MBS:rotations at tEnd= 1.8464514674961 , [ 0.49219906 0.27189991 0.81815805 -0.00305889 0.26831939 -0.00106605] #some graphics settings to get base and tool visualization jointWidth=0.1 jointRadius=0.06 linkWidth=0.1 graphicsBaseList = [graphics.Brick([0,0,-0.35], [0.12,0.12,0.5], graphics.color.grey)] graphicsBaseList +=[graphics.Cylinder([0,0,0], [0.5,0,0], 0.0025, graphics.color.red)] graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0.5,0], 0.0025, graphics.color.green)] graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0,0.5], 0.0025, graphics.color.blue)] graphicsBaseList +=[graphics.Cylinder([0,0,-jointWidth], [0,0,jointWidth], linkWidth*0.5, graphics.colorList[0])] #belongs to first body graphicsBaseList +=[graphics.CheckerBoard([0,0,-0.6], [0,0,1], size=2)] ty = 0.03 tz = 0.04 zOff = -0.05 toolSize= [0.05,0.5*ty,0.06] graphicsToolList = [graphics.Cylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=graphics.color.red)] graphicsToolList+= [graphics.Brick([0,ty,1.5*tz+zOff], toolSize, graphics.color.grey)] graphicsToolList+= [graphics.Brick([0,-ty,1.5*tz+zOff], toolSize, graphics.color.grey)] # here another robot could be loaded; note that the robots sizes and workspaces and zero configuration # differ, so the HTmove may need to be adjusted to be inside the workspace robotDef = ManipulatorPuma560() # robotDef = ManipulatorUR5() # robotDef = ManipulatorPANDA() HTtool = HTtranslate([0,0,0.08]) robot = Robot(gravity=[0,0,-9.81], base = RobotBase(HT=HTtranslate([0,0,0]), visualization=VRobotBase(graphicsData=graphicsBaseList)), tool = RobotTool(HT=HTtool, visualization=VRobotTool(graphicsData=graphicsToolList)), referenceConfiguration = []) #referenceConfiguration created with 0s automatically robot=LinkDict2Robot(robotDef, robot) ik = InverseKinematicsNumerical(robot=robot, useRenderer=False, #flagDebug=True, #jointStiffness=1e4 ) #, useAlternativeConstraints=True) SC = exu.SystemContainer() SC.AttachToRenderEngine() mbs = SC.AddSystem() # the system container holds one or several systems; the inverse kinematics uses a second system container without visualization # 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. #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #configurations and trajectory q0 = [0,0,0,0,0,0] #zero angle configuration # q1 = [0, pi/8, pi*0.5, 0,pi/8,0] #configuration 1 # q2 = [0.8*pi,-0.8*pi, -pi*0.5,0.75*pi,-pi*0.4,pi*0.4] #configuration 2 # q3 = [0.5*pi,0,-0.25*pi,0,0,0] # configuration 3 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #create robot model in mbs #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ mbs.variables['myIkine'] = ik jointHTs = robot.JointHT(q0) HTlastJoint = jointHTs[-1]@HTtool #prescribed motion: #HTmove = HTtranslate([-0.25,0.,0.3]) if motionCase == 1: HTmove = HT(RotationMatrixX(-0.3*pi),[-0.45,0.,0.]) #goes through singularity elif motionCase == 2: HTmove = HT(RotationMatrixX(0.3*pi),[0.,0.,-0.3]) #no singularity else: print('no valid motionCase provided') ## here another motionCase/HTmove could be added logMove = LogSE3(HTmove) rotMove = Skew2Vec(logMove[0:3,0:3]) dispMove = HT2translation(logMove) q0=[0,0,0,0.01,0.02,0.03] [q1, success] = ik.SolveSafe(HTlastJoint@HTmove, q0) if not success: print('[q1, success]=',[q1, success]) [q2, success] = ik.SolveSafe(HTlastJoint@HTmove@HTmove, q1) if not success: print('[q2, success]=',[q2, success]) #initialize for first simulation step: [q, success] = ik.Solve(HTlastJoint, q0) print('[q, success]=',[q, success]) #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #trajectory generated with optimal acceleration profiles, for joint-interpolation: trajectory = Trajectory(initialCoordinates=q0, initialTime=0) trajectory.Add(ProfileConstantAcceleration(q1,1)) trajectory.Add(ProfileConstantAcceleration(q1,1)) trajectory.Add(ProfileConstantAcceleration(q2,1)) trajectory.Add(ProfileConstantAcceleration(q2,1)) #traj.Add(ProfilePTP([1,1],syncAccTimes=False, maxVelocities=[1,1], maxAccelerations=[5,5])) #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #use frame for prescribed TCP: gFrame = [graphics.Frame(HTlastJoint, length=0.3, colors=[graphics.color.lightgrey]*3)] gFrame += [graphics.Frame(HTlastJoint@HTmove, length=0.3, colors=[graphics.color.grey]*3)] gFrame += [graphics.Frame(HTlastJoint@HTmove@HTmove, length=0.3, colors=[graphics.color.dodgerblue]*3)] oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=gFrame))) robotDict = robot.CreateKinematicTree(mbs) oKT = robotDict['objectKinematicTree'] #add sensor for joint coordinates (relative to reference coordinates) sJoints = mbs.AddSensor(SensorNode(nodeNumber=robotDict['nodeGeneric'], storeInternal=True, outputVariableType=exu.OutputVariableType.Coordinates)) sPosTCP = mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=5, localPosition=HT2translation(HTtool), storeInternal=True, outputVariableType=exu.OutputVariableType.Position)) sRotTCP = mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=5, localPosition=HT2translation(HTtool), storeInternal=True, outputVariableType=exu.OutputVariableType.RotationMatrix)) #user function which is called only once per step, speeds up simulation drastically def PreStepUF(mbs, t): if not jointSpaceInterpolation: ik = mbs.variables['myIkine'] if t < 2: tt = min(t,1) T = HTlastJoint@ExpSE3(list(tt*dispMove)+list(tt*rotMove)) elif t < 4: tt=min(t-2,1) T = HTlastJoint@HTmove@ExpSE3(list(tt*dispMove)+list(tt*rotMove)) else: T = HTlastJoint@HTmove@HTmove [q,success]=ik.Solve(T) #, q0) #takes 60 us internally mbs.SetObjectParameter(oKT, 'jointPositionOffsetVector', q) else: #standard trajectory planning: [u,v,a] = trajectory.Evaluate(t) #in case of kinematic tree, very simple operations! mbs.SetObjectParameter(oKT, 'jointPositionOffsetVector', u) mbs.SetObjectParameter(oKT, 'jointVelocityOffsetVector', v) return True mbs.SetPreStepUserFunction(PreStepUF) mbs.Assemble() # assemble the dynamic system #mbs.systemData.Info() #%% setting for visualization SC.visualizationSettings.connectors.showJointAxes = True SC.visualizationSettings.connectors.jointAxesLength = 0.02 SC.visualizationSettings.connectors.jointAxesRadius = 0.002 SC.visualizationSettings.nodes.showBasis = True SC.visualizationSettings.nodes.basisSize = 0.1 SC.visualizationSettings.loads.show = False # shows external loads SC.visualizationSettings.bodies.kinematicTree.showJointFrames = False # shows the frames for each joint of the robot SC.visualizationSettings.openGL.multiSampling=4 SC.visualizationSettings.general.autoFitScene=False SC.visualizationSettings.window.renderWindowSize=[1920,1200] SC.visualizationSettings.general.graphicsUpdateInterval = 0.01 SC.visualizationSettings.openGL.shadow=0.3 SC.visualizationSettings.openGL.perspective=0.5 #traces: SC.visualizationSettings.sensors.traces.listOfPositionSensors = [sPosTCP] SC.visualizationSettings.sensors.traces.listOfTriadSensors =[sRotTCP] SC.visualizationSettings.sensors.traces.showPositionTrace=True SC.visualizationSettings.sensors.traces.showTriads=True SC.visualizationSettings.sensors.traces.showVectors=False SC.visualizationSettings.sensors.traces.showFuture=False SC.visualizationSettings.sensors.traces.triadsShowEvery=5 #%% tEnd = 5 # endtime of the simulation h = 0.002 #500 steps take 0.16 seconds, 0.3ms / step (83% Python + inverse kinematics) # settings for the time integration / simulation simulationSettings = exu.SimulationSettings() #takes currently set values or default values simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h) simulationSettings.timeIntegration.endTime = tEnd simulationSettings.solutionSettings.solutionWritePeriod = 0.02 # determines the timesteps in which the solution is saved; when it is decreased more solutions are saved, which also # enables the solutionViewer to simulationSettings.solutionSettings.sensorsWritePeriod = 0.005 #simulationSettings.solutionSettings.writeSolutionToFile = False # simulationSettings.timeIntegration.simulateInRealtime = True # simulationSettings.timeIntegration.realtimeFactor = 0.025 # slow down simulation to look at simulationSettings.timeIntegration.verboseMode = 1 simulationSettings.displayComputationTime = True # simulationSettings.displayStatistics = True #simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse simulationSettings.timeIntegration.newton.useModifiedNewton = True # solver type simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True #%% useGraphics = True # if useGraphics is false rendering while calculating the solution is dectivated, # the solution is still saved and can be then visualized afterwards for the solution viewer if useGraphics: # start graphics exu.StartRenderer() if 'renderState' in exu.sys: SC.SetRenderState(exu.sys['renderState']) mbs.WaitForUserToContinue() # hte the simulation with the set up simulationsettings is started mbs.SolveDynamic(simulationSettings, #solverType = exudyn.DynamicSolverType.TrapezoidalIndex2, # different solver types can be used depending on the problem showHints=True) if useGraphics: # when graphics are deactivated the renderer does not need to be stopped SC.visualizationSettings.general.autoFitScene = False exu.StopRenderer() #%%++++++++++++++++++++++++++++++++++++++++++++ if True: # import the solution viewer which loads mbs.SolutionViewer() #%%++++++++++++++++++++++++++++++++++++++++++++ q = mbs.GetObjectOutputBody(oKT, exu.OutputVariableType.Coordinates) exu.Print("rotations at tEnd=", VSum(q), ',', q) #%%++++++++++++++++++++++++++++++++++++++++++++ if True: labels = [] for i in range(6): labels += ['joint '+str(i)] # plot data from the sensor sJoints 6 times and take the values 0 to 5 in the steps mbs.PlotSensor(sensorNumbers=[sJoints]*6, components=list(np.arange(6)), yLabel='joint coordinates', labels=labels)