def main(env,options):
"Main example code."
env.Load(options.scene)
target = env.GetKinBody('lever')
if target is None:
target = RaveCreateKinBody(env,'')
target.InitFromBoxes(array([[0,0.1,0,0.01,0.1,0.01]]),True)
target.SetName('lever')
env.Add(target)
T = eye(4)
T[0:3,3] = [-0.2,-0.2,1]
target.SetTransform(T)
robot = env.GetRobots()[0]
robot.SetActiveManipulator('rightarm')
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterizationType.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
with env:
robot.SetActiveDOFs(ikmodel.manip.GetArmIndices())
basemanip=interfaces.BaseManipulation(robot)
taskmanip=interfaces.TaskManipulation(robot)
taskmanip.ReleaseFingers()
robot.WaitForController(0)
with env:
Toffset = eye(4)
Toffset[0:3,3] = array([0,0.2,0])
Ttarget0 = target.GetTransform()
Ttarget1 = dot(Ttarget0,matrixFromAxisAngle([pi/2,0,0]))
# with target.CreateKinBodyStateSaver():
# target.SetTransform(Ttarget1)
# raw_input('as')
Tgrasp0 = dot(matrixFromAxisAngle([pi/2,0,0]),matrixFromAxisAngle([0,pi/2,0]))
Tgrasp0[0:3,3] = dot(Ttarget0,Toffset)[0:3,3]
Tgraspoffset = dot(linalg.inv(Ttarget0),Tgrasp0)
Tgrasp1 = dot(Ttarget1,Tgraspoffset)
# check if ik solutions exist
sol0=ikmodel.manip.FindIKSolution(Tgrasp0,IkFilterOptions.CheckEnvCollisions)
assert(sol0 is not None)
sol1=ikmodel.manip.FindIKSolution(Tgrasp1,IkFilterOptions.CheckEnvCollisions)
assert(sol1 is not None)
traj = RaveCreateTrajectory(env,'')
spec = IkParameterization.GetConfigurationSpecificationFromType(IkParameterizationType.Transform6D,'linear')
traj.Init(spec)
for angle in arange(0,pi/2,0.05):
Ttarget = dot(Ttarget0,matrixFromAxisAngle([angle,0,0]))
Tgrasp = dot(Ttarget,Tgraspoffset)
traj.Insert(traj.GetNumWaypoints(),poseFromMatrix(Tgrasp))
planningutils.RetimeAffineTrajectory(traj,maxvelocities=ones(7),maxaccelerations=5*ones(7))
h=misc.DrawAxes(env,Ttarget0)
basemanip.MoveToHandPosition(matrices=[Tgrasp0])
robot.WaitForController(0)
taskmanip.CloseFingers()
robot.WaitForController(0)
with env:
robot.Grab(target)
print 'planning for turning lever'
planner = RaveCreatePlanner(env,'workspacetrajectorytracker')
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetExtraParameters('<workspacetrajectory>%s</workspacetrajectory>'%traj.serialize(0))
planner.InitPlan(robot,params)
outputtraj = RaveCreateTrajectory(env,'')
success=planner.PlanPath(outputtraj)
assert(success)
# also create reverse the trajectory and run infinitely
trajectories = [outputtraj,planningutils.ReverseTrajectory(outputtraj)]
while True:
for traj in trajectories:
robot.GetController().SetPath(traj)
robot.WaitForController(0)
if options.testmode:
break