I wanto express how to use suction gripper with UR5.
I was going to use setLinkDummy with two dummys.
but error occur, I can't find the reason, can you help?
Thank you
Code: Select all
function sysCall_init()
corout=coroutine.create(coroutineMain)
-- Take a few handles from the dynamic robot:
simBase=sim.getObject('.')
simTip=sim.getObject('./tip')
simTarget=sim.getObject('./target')
simP=sim.getObject('./tcp')
simJoints={}
for i=1,6,1 do
simJoints[i]=sim.getObject('./joint',{index=i-1})
end
-- Prepare an ik group, using the convenience function 'simIK.addIkElementFromScene':
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})
-- Graphs:
graph3dSpace=sim.getObject('/3dSpace')
graphJointSpace=sim.getObject('/jointSpace')
endEffectorVelX=sim.addGraphStream(graph3dSpace,'end effector velocity x','m/s',0,{1,0,0})
endEffectorVelY=sim.addGraphStream(graph3dSpace,'end effector velocity y','m/s',0,{0,1,0})
endEffectorVelZ=sim.addGraphStream(graph3dSpace,'end effector velocity z','m/s',0,{0,0.5,1})
jointVels={}
jointVels[1]=sim.addGraphStream(graphJointSpace,'joint 1 velocity','deg/s',0,{1,0,0})
jointVels[2]=sim.addGraphStream(graphJointSpace,'joint 2 velocity','deg/s',0,{0,1,0})
jointVels[3]=sim.addGraphStream(graphJointSpace,'joint 3 velocity','deg/s',0,{0,0.5,1})
jointVels[4]=sim.addGraphStream(graphJointSpace,'joint 4 velocity','deg/s',0,{1,1,0})
jointVels[5]=sim.addGraphStream(graphJointSpace,'joint 5 velocity','deg/s',0,{1,0,1})
jointVels[6]=sim.addGraphStream(graphJointSpace,'joint 6 velocity','deg/s',0,{0,1,1})
end
function sysCall_sensing()
local vel=sim.getObjectVelocity(simTip)
sim.setGraphStreamValue(graph3dSpace,endEffectorVelX,vel[1])
sim.setGraphStreamValue(graph3dSpace,endEffectorVelY,vel[2])
sim.setGraphStreamValue(graph3dSpace,endEffectorVelZ,vel[3])
for i=1,#simJoints,1 do
sim.setGraphStreamValue(graphJointSpace,jointVels[i],180*sim.getJointVelocity(simJoints[i])/math.pi)
end
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()
-- IK movement data:
local maxIkVel={0.45,0.45,0.45,4.5} -- vx,vy,vz in m/s, Vtheta is rad/s
local maxIkAccel={0.13,0.13,0.13,1.24} -- ax,ay,az in m/s^2, Atheta is rad/s^2
local maxIkJerk={0.1,0.1,0.1,0.2} -- is ignored (i.e. infinite) with RML type 2
local w1=sim.getObjectPose(sim.getObject('/waypoint1'),-1)
local w2=sim.getObjectPose(sim.getObject('/waypoint2'),-1)
local w3=sim.getObjectPose(sim.getObject('/waypoint3'),-1)
local w4=sim.getObjectPose(sim.getObject('/waypoint4'),-1)
local w5=sim.getObjectPose(sim.getObject('/waypoint5'),-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]=1.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
moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,w1,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,w2,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,w3,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,w4,data)
sim.wait(1)
sim.setLinkDummy(SimP,w5)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,w1,data)
end
function sysCall_cleanup()
simIK.eraseEnvironment(ikEnv)
end