UR5正向动力学控制

偏执的太偏执、 2023-01-04 13:13 151阅读 0赞

-- 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)

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70][]![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 1][]![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 2][]![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 3][]

-- 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()

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 4][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 5][]

[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]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 6][]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20210109232211903.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20210109232318498.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 2]: https://img-blog.csdnimg.cn/2021010923244185.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 3]: /images/20221119/f30d6b6aa67b409eb64352f860282e2b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 4]: /images/20221119/a91448bee89649d390fde21e53d32995.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 5]: https://img-blog.csdnimg.cn/20210110125702881.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg_size_16_color_FFFFFF_t_70 6]: https://img-blog.csdnimg.cn/20210110165323615.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2tpc3Nnb29kYnllMjAxMg==,size_16,color_FFFFFF,t_70