def CheckNarrow(waypoint, stepsize):
a = [stepsize, 0, -stepsize]
counter = 0
for dz in a:
# for dy in a:
# for dz in a:
T = waypoint
T[2] = T[2] + dz
robot.SetActiveDOFValues(us.E2Q(T))
if env.CheckCollision(robot):
counter = counter + 1
return counter
def CheckNarrow(waypoint,stepsize):
a = [stepsize,0,-stepsize];
counter = 0;
for dx in a:
for dy in a:
for dz in a:
T = waypoint;
T(0) = T(0)+dx;
T(1) = T(1)+dx;
T(2) = T(2)+dx;
robot.SetActiveDOFValues(us.E2Q(T))
if env.CheckCollision(robot)
counter = counter +1;
return counter;
def FindNarrowPt(path,stepsize):
NarrowPt = [];
for waypoint in path
lvl = CheckNarrow(waypoint,stepsize);
if lvl >= 20:
NarrowPt.append(waypoint);
return NarrowPt
env.Reset()
# load a scene from ProjectRoom environment XML file
env.Load('env/bitreequad.env.xml')
time.sleep(0.1)
# 1) get the 1st robot that is inside the loaded scene
# 2) assign it to the variable named 'robot'
robot = env.GetRobots()[0]
robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.Z|DOFAffine.RotationQuat)
print "DOF"
print robot.GetActiveDOFIndices();
startconfig = [ 4.0,-1.5 ,0.35 ,0.0, 0.0, 0.0 ];
robot.SetActiveDOFValues(us.E2Q(startconfig));
# robot.GetController().SetDesired(robot.GetDOFValues());
# waitrobot(robot);
handles = [];
# raw_input("Press enter to move robot...")
raw_input("Press enter to exit...")
with env:
goalconfig = [-4.3, 0.8, 1 ,0.0 ,0.0 ,0.0];
# goalconfig = [4,0.87,2.09,1.5,0,0];
### YOUR CODE HERE ###
###call your plugin to plan, draw, and execute a path from the current configuration of the left arm to the goalconfig
goalbias = 0.1;
step = 0.3;
# workspace boundary x,y,z
workspaceBound = [-4.5,4.5,-2.2,2.2,0.21,1.54]
print "Successful"
else:
print "Fail"
# robot.SetActiveDOFs([],DOFAffine.X|DOFAffine.Y|DOFAffine.Z|DOFAffine.Rotation3D)
traj = RaveCreateTrajectory(env, '')
config = robot.GetActiveConfigurationSpecification('linear')
config.AddDeltaTimeGroup()
traj.Init(config)
# mypath = [];
newpath = []
offset = 0
for i, point in enumerate(path):
newPoint = us.E2Q(point)
if i == 0:
newpath.append([
newPoint[0], newPoint[1], newPoint[2], newPoint[3],
newPoint[4], newPoint[5], newPoint[6], i * 0.005
])
else:
lastPoint = us.E2Q(path[i - 1])
if (lastPoint[0] == point[0] and lastPoint[1] == point[1]
and lastPoint[2] == point[2]):
offset = offset + 1
pass
else:
newpath.append([
newPoint[0], newPoint[1], newPoint[2], newPoint[3],
robot3 = env.GetRobots()[2]
robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.Z
| DOFAffine.RotationQuat)
robot2.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.Z
| DOFAffine.RotationQuat)
robot3.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.Z
| DOFAffine.RotationQuat)
# print robot.GetActiveDOFValues()
# raw_input("Press enter to move robot...")
# qt = tf.quaternion_from_euler(0.5,0.5,0.75,'rzxz')
# startconfig = [4.0,-1.5 ,0.2] + list(qt)
# print startconfig
startconfig = [4.0, -1.5, 0.2, 0.0, 0.0, 0.0]
robot.SetActiveDOFValues(us.E2Q(startconfig))
# robot.GetController().SetDesired(robot.GetDOFValues());
# waitrobot(robot);
waitrobot(robot)
print "test update state"
# s1 = [1,1,1,1,0,0,0,0.2,0.2,0.2,0.1,0.1,-0.1]
avf = 1.85 * 9.8 / 4
u = [-0.5 * avf, 2 * avf, -0.5 * avf, 3 * avf]
ts = 0.02
t = range(0, 150)
speed = 1