def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
self.environment.removePermanentParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for p in path:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
particle_joint_values[:] = pjvs
self.environment.plotPermanentParticles(particle_joint_values,
particle_joint_colors)
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
self.environment.removePermanentParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for p in path:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
particle_joint_values[:] = pjvs
self.environment.plotPermanentParticles(particle_joint_values,
particle_joint_colors)
def play_states(self, states, col, particles):
for i in xrange(len(states)):
cjvals = v_double()
cjvels = v_double()
print "states[i] " + str(states[i])
cjvals_arr = [states[i][j] for j in xrange(len(states[i]) / 2)]
cjvels_arr = [
states[i][j]
for j in xrange(len(states[i]) / 2, len(states[i]))
]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
if i > 1 and len(particles) > 0:
for p in particles[i - 1]:
particle = v_double()
particle_color = v_double()
particle[:] = [p[k] for k in xrange(len(p) / 2)]
particle_color[:] = [0.5, 0.5, 0.5, 0.5]
particle_joint_values.append(particle)
particle_joint_colors.append(particle_color)
print "updating robot values"
self.environment.updateRobotValues(cjvals, cjvels,
particle_joint_values,
particle_joint_colors)
print "updated robot values"
'''for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor()) '''
self.environment.setKinBodiesDefaultColor()
try:
if col[i] == True:
diffuse_col = v_double()
ambient_col = v_double()
diffuse_col[:] = [0.5, 0.0, 0.0, 0.0]
ambient_col[:] = [0.8, 0.0, 0.0, 0.0]
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(), diffuse_col,
ambient_col)
except:
pass
time.sleep(0.3)
def play_states(self, states, col, particles):
for i in xrange(len(states)):
cjvals = v_double()
cjvels = v_double()
print "states[i] " + str(states[i])
cjvals_arr = [states[i][j] for j in xrange(len(states[i]) / 2)]
cjvels_arr = [states[i][j] for j in xrange(len(states[i]) / 2, len(states[i]))]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
if i > 1 and len(particles) > 0:
for p in particles[i-1]:
particle = v_double()
particle_color = v_double()
particle[:] = [p[k] for k in xrange(len(p) / 2)]
particle_color[:] = [0.5, 0.5, 0.5, 0.5]
particle_joint_values.append(particle)
particle_joint_colors.append(particle_color)
print "updating robot values"
self.environment.updateRobotValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_colors)
print "updated robot values"
'''for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor()) '''
self.environment.setKinBodiesDefaultColor()
try:
if col[i] == True:
diffuse_col = v_double()
ambient_col = v_double()
diffuse_col[:] = [0.5, 0.0, 0.0, 0.0]
ambient_col[:] = [0.8, 0.0, 0.0, 0.0]
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
diffuse_col,
ambient_col)
except:
pass
time.sleep(0.3)
def prog(joint_angles, sensor_name):
joint_angles_v = v_double()
joint_velocities = v_double()
particle_joint_values = v2_double()
joint_angles_v[:] = [joint_angles[i] for i in xrange(len(joint_angles))]
joint_velocities[:] = [0.0, 0.0, 0.0]
env.getRobot().setState(joint_angles_v, joint_velocities)
env.transformSensorSensorLink(joint_angles_v, sensor_name)
env.updateRobotValues(joint_angles_v,
joint_angles_v,
particle_joint_values,
particle_joint_values)
time.sleep(1)
env.getSensorManager().activateSensor(sensor_name)
time.sleep(1)
env.drawBoxes()
time.sleep(1)
env.getSensorManager().deactivateSensor(sensor_name)
time.sleep(1.0)
def test_collision(env):
robot = env.getRobot()
collision_manager = env.getCollisionManager()
joint_angles_v = v_double()
joint_angles_v_2 = v_double()
joint_angles_v[:] = [0.2, 0.0, 0.0]
joint_angles_v_2[:] = [0.2, 0.0, 0.0]
particle_joint_values = v2_double()
env.updateRobotValues(joint_angles_v,
joint_angles_v,
particle_joint_values,
particle_joint_values)
robot_collision_objects_start = robot.createRobotCollisionObjects(joint_angles_v)
robot_collision_objects_goal = robot.createRobotCollisionObjects(joint_angles_v_2)
for i in xrange(len(robot_collision_objects_start)):
in_collision = collision_manager.inCollisionContinuousEnvironment([robot_collision_objects_start[i],
robot_collision_objects_goal[i]])
print in_collision
time.sleep(100)
