UR5正向动力学控制

-- This is a threaded script, and is just an example!

jointHandles={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
    jointHandles[i]=simGetObjectHandle('UR5_joint'..i)
end

-- Set-up some of the RML vectors:
vel=180
accel=40
jerk=80
currentVel={0,0,0,0,0,0,0}
currentAccel={0,0,0,0,0,0,0}
maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
targetVel={0,0,0,0,0,0}

targetPos3={180*math.pi/180,0*math.pi/180,0*math.pi/180,0*math.pi/180,0*math.pi/180,0*math.pi/180}
simRMLMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos3,targetVel)

 

-- This is a threaded script, and is just an example!

threadFunction=function()

   while simGetSimulationState()~=sim_simulation_advancing_abouttostop do
   --read joints data from simulink

   result,data=simExtShareMemoryRead(UR5_data)
   UR5_torque=simUnpackFloats(data,0,6,0)
   for i=1,6,1 do
        simRMLMoveToJointPositions(UR5_jointhandle,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,UR5_torque,targetVel)
   end

  --show ur5 joint torque
   simAuxiliaryConsolePrint(UR5_consoleHandle,"JOINT1: "..UR5_torque[1].."\n")
   simAuxiliaryConsolePrint(UR5_consoleHandle,"JOINT2: "..UR5_torque[2].."\n")
   simAuxiliaryConsolePrint(UR5_consoleHandle,"JOINT3: "..UR5_torque[3].."\n")
   simAuxiliaryConsolePrint(UR5_consoleHandle,"JOINT4: "..UR5_torque[4].."\n")
   simAuxiliaryConsolePrint(UR5_consoleHandle,"JOINT5: "..UR5_torque[5].."\n")
   simAuxiliaryConsolePrint(UR5_consoleHandle,"JOINT6: "..UR5_torque[6].."\n")

 --send ur5 joint angle and velocity to simulink
   local joint_q={-1,-1,-1,-1,-1,-1}
   for i=1,6,1 do
      joint_q[i]=simGetJointPosition(UR5_jointhandle[i])--angle unit:rad
      temp,joint_q[6+i]=simGetObjectFloatParameter(UR5_jointhandle[i],2012)--velocity uint:rad
   end
    --joint_q[2]=joint_q[2]+3*math.pi/2;
    --joint_q[4]=joint_q[4]+math.pi/2;
    --joint_q[6]=joint_q[6]+math.pi;
   joint_data_export=simPackFloats(joint_q)
   simExtShareMemoryWrite(UR5_floatMem_out1,joint_data_export)

--send ur5_force_sensor data to simulink

   force_data={-1,-1,-1,-1,-1,-1}

   result,force_data[1]=simGetObjectFloatParameter(force_sensor_handle,5000) --f x
   result,force_data[2]=simGetObjectFloatParameter(force_sensor_handle,5001) --f y
   result,force_data[3]=simGetObjectFloatParameter(force_sensor_handle,5002)  --f z
   result,force_data[4]=simGetObjectFloatParameter(force_sensor_handle,5003)  --t alpha
   result,force_data[5]=simGetObjectFloatParameter(force_sensor_handle,5004)  --t beta
   result,force_data[6]=simGetObjectFloatParameter(force_sensor_handle,5005)  --t gamma

   force_data_export=simPackFloats(force_data)
   simAuxiliaryConsolePrint(UR5_consoleHandle,"force_sensor x: "..force_data[1].."\n")
   simExtShareMemoryWrite(UR5_floatMem_out2,force_data_export) 

   simSwitchThread()
   end
end

simSetThreadSwitchTiming(20) -- Default timing for automatic thread switching
simExtShareMemoryClose()

result,UR5_data=simExtShareMemoryOpen("UR5_entrada_1",4*6)
result,UR5_floatMem_out1=simExtShareMemoryOpen("UR5_salida_1",4*6*2)
result,UR5_floatMem_out2=simExtShareMemoryOpen("force_cencer_salida_3",4*6)

UR5_jointhandle={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
    UR5_jointhandle[i]=simGetObjectHandle('UR5_joint'..i)
end

-- Set-up some of the RML vectors:
vel=180
accel=40
jerk=80
currentVel={0,0,0,0,0,0,0}
currentAccel={0,0,0,0,0,0,0}
maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
targetVel={0,0,0,0,0,0}

force_sensor_handle=simGetObjectHandle('UR5_force_sensor')
UR5_consoleHandle=simAuxiliaryConsoleOpen('UR5_data',200,1,nil,nil,nil,nil)

-- Here we execute the regular thread code:
res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)
if not res then
	simAddStatusbarMessage('Lua runtime error: '..err)    
end

-- Put some clean-up code here:
simExtShareMemoryClose()

 

 

 

[0.1623 -0.3938 0.9047 0.1377;-0.5888 0.6972 0.4091 -0.0699;-0.7919 -0.5991 -0.118 -0.5412;0 0 0 1.0000]

//第一次自己定义
[-0.960663680652984,0.275587492022499,0.0343049109459684,-0.317567497001228;-0.0342208533572619,0.00511503045417374,-0.999401205552082,-0.197009787885241;-0.275597942426577,-0.961262383901608,0.00451701516883886,-0.343287336468640;0 0 0 1]

//第二次定义
[-0.0349 -0.9994 0 -0.0254;0.9994 -0.0349 0	-0.1096;0 0 1 -0.2513;0 0 0 1]