This is the code I'm using, it comes from the IK scenes and considers two different logical condition:
Code: Select all
function sysCall_init()
corout=coroutine.create(coroutineMain)
-- Take a few handles from the dynamic robot:
simBase=sim.getObjectHandle('UR10')
simTip=sim.getObjectHandle('UR10_tip')
simTarget=sim.getObjectHandle('UR10_target')
Safety_Sensor_Handle=sim.getObjectHandle('Safety_Zone_Sensor')
Human_Handle=sim.getObjectHandle('Bill_Path')
BillCollection=sim.createCollection(0)
sim.addItemToCollection(BillCollection,sim.handle_tree,Human_Handle,0)
simJoints={}
for i=1,6,1 do
simJoints[i]=sim.getObjectHandle('UR10_joint'..i)
end
-- Following path task
ikEnv=simIK.createEnvironment()
ikGroup=simIK.createIkGroup(ikEnv)
simIK.setIkGroupCalculation(ikEnv,ikGroup,simIK.method_damped_least_squares,0.01,10)
local ikElement=simIK.addIkElementFromScene(ikEnv,ikGroup,simBase,simTip,simTarget,simIK.constraint_pose)
simIK.setIkElementPrecision(ikEnv,ikGroup,ikElement,{0.0005,0.005*math.pi/180})
end
function sysCall_sensing()
local vel=sim.getObjectVelocity(simTip)
-- Proximity sensor read
Safetyresult,Safetydistance,SafetydetectedPoint,SafetydetectedObjectHandle,SafetydetectedSurfaceNormalVector=sim.handleProximitySensor(Safety_Sensor_Handle)
SafetyZoneresult,SafetyZonedistance=sim.readProximitySensor(Safety_Sensor_Handle)
local txt='SafetyZoneresult is '..SafetyZoneresult
print(txt)
end
function sysCall_actuation()
if coroutine.status(corout)~='dead' then
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
end
end
function moveToPoseCallback(pose,velocity,accel,auxData)
sim.setObjectPose(auxData.target,-1,pose)
simIK.applyIkEnvironmentToScene(auxData.ikEnv,auxData.ikGroup)
end
function moveToPose_viaIK(maxVelocity,maxAcceleration,maxJerk,targetPose,auxData)
local currentPose=sim.getObjectPose(auxData.tip,-1)
return sim.moveToPose(-1,currentPose,maxVelocity,maxAcceleration,maxJerk,targetPose,moveToPoseCallback,auxData,nil)
end
function moveToConfigCallback(config,velocity,accel,auxData)
for i=1,#auxData.joints,1 do
local jh=auxData.joints[i]
if sim.getJointMode(jh)==sim.jointmode_force and sim.isDynamicallyEnabled(jh) then
sim.setJointTargetPosition(jh,config[i])
else
sim.setJointPosition(jh,config[i])
end
end
end
function moveToConfig_viaFK(maxVelocity,maxAcceleration,maxJerk,goalConfig,auxData)
local startConfig={}
for i=1,#auxData.joints,1 do
startConfig[i]=sim.getJointPosition(auxData.joints[i])
end
sim.moveToConfig(-1,startConfig,nil,nil,maxVelocity,maxAcceleration,maxJerk,goalConfig,nil,moveToConfigCallback,auxData,nil)
end
function coroutineMain()
-- Proximity sensor logic conditions
if SafetyZoneresult==1 then
HumanInDanger=true
else
HumanInDanger=false
end
-- IK movement data:
local maxIkVel={0.15,0.15,0.15,1.5} -- vx,vy,vz in m/s, Vtheta is rad/s
local maxIkAccel={0.03,0.03,0.03,0.24} -- ax,ay,az in m/s^2, Atheta is rad/s^2
local maxIkJerk={0.01,0.01,0.01,0.02} -- is ignored (i.e. infinite) with RML type 2
local minIkVel={0.05,0.05,0.05,0.5} -- vx,vy,vz in m/s, Vtheta is rad/s
local minIkAccel={0.01,0.01,0.01,0.01} -- ax,ay,az in m/s^2, Atheta is rad/s^2
local minIkJerk={0.01,0.01,0.01,0.02} -- is ignored (i.e. infinite) with RML type 2
local Target_1=sim.getObjectPose(sim.getObjectHandle('Object_Target_1'),-1)
local Target_2=sim.getObjectPose(sim.getObjectHandle('Object_Target_2'),-1)
local Target_3=sim.getObjectPose(sim.getObjectHandle('Object_Target_3'),-1)
local Target_4=sim.getObjectPose(sim.getObjectHandle('Object_Target_4'),-1)
-- FK movement data:
local initConf={}
local maxConfVel={}
local maxConfAccel={}
local maxConfJerk={}
for i=1,#simJoints,1 do
initConf[i]=sim.getJointPosition(simJoints[i])
maxConfVel[i]=45*math.pi/180
maxConfAccel[i]=0.15
maxConfJerk[i]=0.4
end
-- Execute the movement here:
local data={}
data.ikEnv=ikEnv
data.ikGroup=ikGroup
data.tip=simTip
data.target=simTarget
data.joints=simJoints
-- Wait until the human reaches the TP
Robot_Start=sim.waitForSignal('Human_TP')
if not HumanInDanger and Robot_Start then
sim.setJointTargetVelocity(simJoints[1],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[2],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[3],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[4],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[5],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[6],maxIkVel[1])
moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_1,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_2,data)
-- Human restart condition
sim.setIntegerSignal('Robot_Finish',2)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_3,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_4,data)
end
if HumanInDanger then
sim.setJointTargetVelocity(simJoints[1],minIkVel[1])
sim.setJointTargetVelocity(simJoints[2],minIkVel[1])
sim.setJointTargetVelocity(simJoints[3],minIkVel[1])
sim.setJointTargetVelocity(simJoints[4],minIkVel[1])
sim.setJointTargetVelocity(simJoints[5],minIkVel[1])
sim.setJointTargetVelocity(simJoints[6],minIkVel[1])
moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
moveToPose_viaIK(minIkVel,minIkAccel,minIkJerk,Target_1,data)
moveToPose_viaIK(minIkVel,minIkAccel,minIkJerk,Target_2,data)
moveToPose_viaIK(minIkVel,minIkAccel,minIkJerk,Target_3,data)
moveToPose_viaIK(minIkVel,minIkAccel,minIkJerk,Target_4,data)
end
end
function sysCall_cleanup()
simIK.eraseEnvironment(ikEnv)
end
I know that this code doesn't work because I should stop the robot motion and the restart it with different values of speed/acceleration, but I don't know how to do that.
It would also be better to just slow down the robot without stopping it, but I don't know how to do that since this is the only IK code that I found as reference.
There's a way to stop/slow down the robot when the condition Humanindanger is set to "true"?