Hello,
you have several problems with your scene: first you should decide if you want/need a dynamically enabled scene, or if a kinematic scene is enough. Currently you have most elements that are dynamically enabled.
For a kinematic scene, make sure all shapes are non-respondable and static. Your joints are already all in kinematic mode (correct in that situation).
Then start with a single loop-closing constraint, and make the motor rotate:
Code: Select all
function sysCall_init()
-- Take a few handles from the scene:
local simBase=sim.getObject('.')
local simClosureTip=sim.getObject('./ClosureTip')
local simClosureTarget=sim.getObject('./ClosureTarget')
local simTip=sim.getObject('./Tip')
local simTarget=sim.getObject('./Target')
simMotorJoint=sim.getObject('./Motor1')
ikEnv = simIK.createEnvironment()
ikGroup=simIK.createGroup(ikEnv)
simIK.setGroupCalculation(ikEnv,ikGroup,simIK.method_damped_least_squares,0.1,100)
local ikElement,simToIkObjectMap=simIK.addElementFromScene(ikEnv,ikGroup,simBase,simClosureTip,simClosureTarget,simIK.constraint_x|simIK.constraint_y|simIK.constraint_orientation)
simIK.setJointMode(ikEnv,simToIkObjectMap[simMotorJoint],simIK.jointmode_passive) -- make sure that joint will act as 'rigid' during IK calculations
end
function sysCall_actuation()
sim.setJointPosition(simMotorJoint,sim.getJointPosition(simMotorJoint)+0.01)
simIK.handleGroup(ikEnv,ikGroup,{syncWorlds=true})
end
then add the second constraint:
Code: Select all
function sysCall_init()
-- Take a few handles from the scene:
local simBase=sim.getObject('.')
local simClosureTip=sim.getObject('./ClosureTip')
local simClosureTarget=sim.getObject('./ClosureTarget')
local simTip=sim.getObject('./Tip')
local simTarget=sim.getObject('./Target')
simMotorJoint=sim.getObject('./Motor1')
ikEnv = simIK.createEnvironment()
ikGroup=simIK.createGroup(ikEnv)
simIK.setGroupCalculation(ikEnv,ikGroup,simIK.method_damped_least_squares,0.1,100)
local ikElement,simToIkObjectMap=simIK.addElementFromScene(ikEnv,ikGroup,simBase,simClosureTip,simClosureTarget,simIK.constraint_x|simIK.constraint_y|simIK.constraint_orientation)
simIK.setJointMode(ikEnv,simToIkObjectMap[simMotorJoint],simIK.jointmode_passive) -- make sure that joint will act as 'rigid' during IK calculations
local ikElement,simToIkObjectMap=simIK.addElementFromScene(ikEnv,ikGroup,simBase,simTip,simTarget,simIK.constraint_x|simIK.constraint_y)
end
function sysCall_actuation()
sim.setJointPosition(simMotorJoint,sim.getJointPosition(simMotorJoint)+0.01)
simIK.handleGroup(ikEnv,ikGroup,{syncWorlds=true,allowError=true})
end
notice that now we have as option for simIK.handleGroup
allowError=true, because the system is now overconstrained and often the tip can't reach the target.
Cheers