- generate the destination pose i want to reach from a non threated child script and set the command to the robot trought an integer signal
- associate to a minipulator a non threated child script that is always listening to the previous signal in order to start its mission. Once the command is given, it has to move from its actual config to the table6_source position through a straight line path. For simplicity, no collisions are considered.
The child script associated to the manipulator is (avoiding initialization part):
Code: Select all
function sysCall_actuation()
if stateMan == 0 then
if sim.getIntegerSignal('Do Manipulator Mission') == 1 then
local boxPosHandle = sim.getObjectHandle('TestTarget')
local pos=sim.getObjectPosition(boxPosHandle,-1)
local orientation=sim.getObjectOrientation(boxPosHandle,-1)
table6_source={pos[1],pos[2],pos[3], orientation[1],orientation[2],orientation[3]}
stateMan = 1
end
end
if stateMan == 1 then
--move orientation target to the dst position
print('move orientation target to ',table6_source)
sim.setObjectPosition(orientationTargetHandle,-1,{table6_source[1],table6_source[2],table6_source[3]})
--shift along desired axis
local m = sim.getObjectMatrix(orientationTargetHandle,-1)
m = getMatrixShiftedAlongAxis(m,-0.8,2) -- shoft along the z axis, a personal modification about original function
sim.setObjectPosition(orientationTargetHandle,-1,{m[4],m[8],m[12]}) -- this position is checked, it's ok!
-- Compute a straight-line path from current config to pose m:
path=generateIkPath(getConfig(),m,ikSteps)
if path == nil then
print('null path')
end
-- Follow the path:
followPath(path)
stateMan = 2
end
if stateMan ==2 then
print('end')
sim.setIntegerSignal('Do Manipulator Mission',0)
stateMan = 0
end
end
Code: Select all
generateIkPath=function(startConfig,goalPose,steps)
-- Generates (if possible) a linear, collision free path between a robot config and a target pose
local currentConfig=getConfig()
setConfig(startConfig)
sim.setObjectMatrix(ikTarget,-1,goalPose)
ikCalculationResult,jointValues=sim.checkIkGroup(ikGroup,jh,nil)
print('results from ik check',ikCalculationResult,jointValues)
local c=sim.generateIkPath(ikGroup,jh,steps,nil,nil)
setConfig(currentConfig)
return c
end
What am i missing?
Cheers,
Davide