if (sim_call_type==sim_childscriptcall_initialization) then-- Put some initialization code heretargetHandle = simGetObjectHandle('ikTarget')endif (sim_call_type==sim_childscriptcall_actuation) then-- Put your main ACTUATION code hereendif (sim_call_type==sim_childscriptcall_sensing) then-- Put your main SENSING code here-- Read the keyboard messages (make sure the focus is on the main window, scene view):message, auxiliaryData = simGetSimulatorMessage()if (message == sim_message_keypress) thenif (auxiliaryData[1]==119) then-- W keylocal p = simGetObjectPosition(targetHandle, -1)p[1] = p[1] - 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==115) then-- S keylocal p = simGetObjectPosition(targetHandle, -1)p[1] = p[1] + 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==2007) then-- up keylocal p = simGetObjectPosition(targetHandle, -1)p[3] = p[3] + 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==2008) then-- down keylocal p = simGetObjectPosition(targetHandle, -1)p[3] = p[3] - 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==2009) then-- left keylocal p = simGetObjectPosition(targetHandle, -1)p[2] = p[2] - 0.001simSetObjectPosition(targetHandle, -1, p)endif (auxiliaryData[1]==2010) then-- right keylocal p = simGetObjectPosition(targetHandle, -1)p[2] = p[2] + 0.001simSetObjectPosition(targetHandle, -1, p)endend
endif (sim_call_type==sim_childscriptcall_cleanup) then-- Put some restoration code hereend

View Code

if (sim_call_type==sim_childscriptcall_initialization) then-- Check if the plugin is loaded:moduleName=0moduleVersion=0index=0pluginNotFound=truewhile moduleName domoduleName,moduleVersion=simGetModuleName(index)if (moduleName=='CHAI3D') thenpluginNotFound=falseendindex=index+1endif (pluginNotFound) thensimDisplayDialog('Error','CHAI3D plugin was not found, or was not correctly initialized (v_repExtCHAI3D.dll).',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})else-- Start the device:local toolRadius = 0.001 -- the radius of the toollocal workspaceRadius = 0.2 -- the workspace radiusif simExtCHAI3D_start(0, toolRadius,workspaceRadius) ~= 1 thensimDisplayDialog('Error','Device failed to initialize.',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})elseCHAI3DPluginInitialized = trueendendtargetHandle = simGetObjectHandle('ikTarget')deltaPos = {0, 0, 0}counter = 0ratio = 50endif (sim_call_type==sim_childscriptcall_actuation) thenif buttonState ==1 then  -- press the buttonlocal p = simGetObjectPosition(targetHandle, -1) -- get the target position-- add increment of the tool tipp[1] = p[1] + deltaPos[1] / ratio                  p[2] = p[2] + deltaPos[2] / ratiop[3] = p[3] + deltaPos[3] / ratiosimSetObjectPosition(targetHandle, -1, p)  -- move to the absolute positionend
endif (sim_call_type==sim_childscriptcall_sensing) thenif CHAI3DPluginInitialized then-- Read the current position of the cursor:local currentToolPosition = simExtCHAI3D_readPosition(0)-- Read the buttons of the device:buttonState = simExtCHAI3D_readButtons(0)counter = counter + 1      -- increase the counterif counter % 30 == 1 then  -- keep the valueprevToolPosition = currentToolPositionendif counter % 30 == 0 then  -- calculate tool tip incrementdeltaPos[1] = currentToolPosition[1] - prevToolPosition[1]  -- X-axis incrementdeltaPos[2] = currentToolPosition[2] - prevToolPosition[2]  -- Y-axis incrementdeltaPos[3] = currentToolPosition[3] - prevToolPosition[3]  -- Z-axis incrementcounter = 0  -- reset counterlocal info = string.format("CurrentPosition:%.2f,%.2f,%.2f  DeltaPosition:%.2f,%.2f,%.2f",currentToolPosition[1],currentToolPosition[2],currentToolPosition[3],deltaPos[1],deltaPos[2],deltaPos[3])simAddStatusbarMessage(info)endend
endif (sim_call_type==sim_childscriptcall_cleanup) thenif CHAI3DPluginInitialized then-- Disconnects all devices and removes all objects from the scene
        simExtCHAI3D_reset()end
end

import math
import numpy as np# link length
L1 = 1
L2 = 1gamma = 1       # step size
lamda = 0.2     # damping constant (DLS-method)
stol = 1e-3     # tolerance
nm = 10         # initial error
count = 0       # iteration count
ilimit = 20     # maximum iteration# numerical method for inverse kinematics
method = 'Pseudo Inverse'  # 'Pseudo Inverse', 'DLS', ...# initial joint value
q = np.array([0, 0, math.pi/2, 0]) # [theta1, d1, theta2, theta3]# target position
target_pos = np.array([1, 0, 2])   # [x,y,z]while True:if(nm > stol):# forward kinematics:x = np.array([math.cos(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.sin(q[0])*math.sin(q[3]),\math.cos(q[2])*math.sin(q[0])*(L1+L2*math.cos(q[3]))-L2*math.cos(q[0])*math.sin(q[3]),\q[1]+(L1+L2*math.cos(q[3]))*math.sin(q[2])])# compute errorerror = target_pos - x# compute JacobianJ11 = -math.sin(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.cos(q[0])*math.sin(q[3])J12 = 0J13 = -math.sin(q[2])*math.cos(q[0])*(L1+L2*math.cos(q[3]))J14 = L2*(math.sin(q[0])*math.cos(q[3])-math.cos(q[0])*math.cos(q[2])*math.sin(q[3]))J21 = math.cos(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.sin(q[0])*math.sin(q[3])J22 = 0J23 = -math.sin(q[0])*math.sin(q[2])*(L1+L2*math.cos(q[3]))J24 = -L2*(math.cos(q[0])*math.cos(q[3])+math.sin(q[0])*math.cos(q[2])*math.sin(q[3]))J31 = 0J32 = 1J33 = math.cos(q[2])*(L1+L2*math.cos(q[3]))J34 = -L2*math.sin(q[2])*math.sin(q[3])J = np.array([[J11,J12,J13,J14],[J21,J22,J23,J24],[J31,J32,J33,J34]])if method == 'Pseudo Inverse': # Pseudo Inverse MethodJ_pseudo = np.dot(J.transpose(), np.linalg.inv(J.dot(J.transpose()))) # compute pseudo inversedq = gamma * J_pseudo.dot(error)if method == 'DLS':# Damped Least Squares Methodf = np.linalg.solve(J.dot(J.transpose())+lamda**2*np.identity(3), error)dq = np.dot(J.transpose(), f)# update joint position   q = q + dqnm = np.linalg.norm(error)count = count + 1if count > ilimit:print "Solution wouldn't converge!"breakelse:# print resultprint 'theta1=' + str(q[0]*180/math.pi)+' d1='+str(q[1]) + ' theta2=' + str((q[2]-math.pi/2)*180/math.pi)+' theta3=' + str(q[3]*180/math.pi)print 'Current position: %.2f, %.2f, %.2f' %(x[0],x[1],x[2])    print str(count)+' iterations'+'  err:'+str(nm)break