def main(env,options):
"Main example code."
env.Load(options.scene)
robot = env.GetRobots()[0]
if options.manipname is not None:
robot.SetActiveManipulator(options.manipname)
with env:
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
basemanip = interfaces.BaseManipulation(robot)
taskmanip = interfaces.TaskManipulation(robot)
robot.SetJointValues([-0.97],ikmodel.manip.GetGripperIndices())
Tstart = array([[ -1, 0, 0, 2.00000000e-01], [ 0,0, 1, 6.30000000e-01], [ 0, 1 , 0, 5.50000000e-02], [ 0,0,0,1]])
sol = ikmodel.manip.FindIKSolution(Tstart,IkFilterOptions.CheckEnvCollisions)
robot.SetDOFValues(sol,ikmodel.manip.GetArmIndices())
#basemanip.MoveToHandPosition([Tstart],maxiter=1000,maxtries=1,seedik=4)
#robot.WaitForController(0)
if len(ikmodel.manip.GetGripperIndices()) > 0:
taskmanip.CloseFingers()
robot.WaitForController(0)
with env:
target = env.GetKinBody('cylinder_green_3')
robot.Grab(target)
updir = array((0,0,1))
success = basemanip.MoveHandStraight(direction=updir,stepsize=0.01,minsteps=1,maxsteps=40)
robot.WaitForController(0)
success = basemanip.MoveHandStraight(direction=-updir,stepsize=0.01,minsteps=1,maxsteps=40)
robot.WaitForController(0)
# test verification with offset (should succeed)
T = ikmodel.manip.GetTransform()
T[1,3] += 0.1
success = basemanip.MoveHandStraight(direction=updir,starteematrix=T,stepsize=0.01,minsteps=1,maxsteps=20)
robot.WaitForController(0)
print 'checking for existance of trajectories with random queries of moving in a straight line'
armlength = 0
armjoints = [j for j in robot.GetDependencyOrderedJoints() if j.GetJointIndex() in ikmodel.manip.GetArmIndices()]
eetrans = ikmodel.manip.GetTransform()[0:3,3]
for j in armjoints[::-1]:
armlength += sqrt(sum((eetrans-j.GetAnchor())**2))
eetrans = j.GetAnchor()
stepsize=0.01
failedattempt = 0
while True:
with env:
#Tee = dot(ikmodel.manip.GetTransform(),matrixFromAxisAngle(random.rand(3)-0.5,0.2*random.rand()))
Tee = matrixFromAxisAngle(random.rand(3)-0.5,pi*random.rand())
direction = random.rand(3)-0.5
direction /= linalg.norm(direction)
x = random.rand(3)-0.5
length = 0.6*random.rand()*armlength
Tee[0:3,3] = eetrans + x/linalg.norm(x)*(armlength-length)
maxsteps=int(length/stepsize)+1
minsteps = maxsteps/2
h = env.drawlinelist(array([Tee[0:3,3],Tee[0:3,3]+direction*maxsteps*stepsize]),1)
try:
success = basemanip.MoveHandStraight(direction=direction,starteematrix=Tee,stepsize=stepsize,minsteps=minsteps,maxsteps=maxsteps)
params = (direction,Tee)
print '%d failed attemps before found'%failedattempt,repr(params)
failedattempt = 0
h = env.drawlinelist(array([Tee[0:3,3],Tee[0:3,3]+direction*maxsteps*stepsize]),4,[0,0,1])
robot.WaitForController(0)
except planning_error,e:
failedattempt += 1