def setup(self,
robot,
obstacles,
max_velocity,
delta_t,
planning_algorithm,
path_timeout,
continuous_collision):
"""
Generate the obstacles
"""
self.robot = robot
logging.info("IKSolutionGenerator: Setup")
self.link_dimensions = v2_double()
self.robot.getActiveLinkDimensions(self.link_dimensions)
self.path_planner = PathPlanner(robot,
delta_t,
continuous_collision,
max_velocity,
1.0,
False,
True,
planning_algorithm)
self.path_planner.setup()
self.path_planner.setObstacles(obstacles)
self.obstacles = obstacles
self.path_timeout = path_timeout
print "setup"
def update_viewer(self,
x,
x_estimate,
z,
control_duration=None,
colliding_obstacle=None):
""" Update the viewer to display the state 'x' of the robot
Optionally sleep for 'control_duration'
"""
if self.show_viewer:
cjvals = v_double()
cjvels = v_double()
cjvals_arr = [x[i] for i in xrange(len(x) / 2)]
cjvels_arr = [x[i] for i in xrange(len(x) / 2, len(x))]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
pjv = v_double()
pjv[:] = [z[i] for i in xrange(len(z))]
pjv_estimate = v_double()
pjv_estimate[:] = [x_estimate[i] for i in xrange(len(x_estimate))]
particle_joint_values.append(pjv)
particle_joint_values.append(pjv_estimate)
particle_joint_colors = v2_double()
pja = v_double()
pja[:] = [0.5, 0.5, 0.9, 0.0]
pja_estimate = v_double()
pja_estimate[:] = [0.2, 0.8, 0.2, 0.0]
particle_joint_colors.append(pja)
particle_joint_colors.append(pja_estimate)
self.robot.updateViewerValues(cjvals, cjvels,
particle_joint_values,
particle_joint_colors)
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor())
if colliding_obstacle != None:
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]
self.robot.setObstacleColor(colliding_obstacle.getName(),
diffuse_col, ambient_col)
time.sleep(control_duration)
time.sleep(1.0)
def show_nominal_path(self, robot, path):
if self.show_viewer:
robot.removePermanentViewerParticles()
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
robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
print "Permanent particles added"
def show_nominal_path(self, robot, path):
if self.show_viewer:
robot.removePermanentViewerParticles()
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
robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
print "Permanent particles added"
def visualize_x(self, robot, x, color=None):
cjvals = v_double()
cjvels = v_double()
cjvals[:] = [x[i] for i in xrange(len(x) / 2)]
cjvels[:] = [x[i] for i in xrange(len(x) / 2, len(x))]
particle_joint_values = v2_double()
robot.updateViewerValues(cjvals, cjvels, particle_joint_values,
particle_joint_values)
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
if self.show_viewer:
self.robot.removePermanentViewerParticles()
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.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
if self.show_viewer:
self.robot.removePermanentViewerParticles()
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.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def show_state_and_cov(self, state, cov, samples):
joint_values = v_double()
joint_values[:] = [state[i] for i in xrange(len(state) / 2)]
joint_velocities = v_double()
joint_velocities[:] = [state[i] for i in xrange(len(state) / 2, len(state))]
particles = v2_double()
particle_joint_colors = v2_double()
#samples = self.sample_multivariate_normal(state, cov, 50)
for s in samples:
particle = v_double()
particle_color = v_double()
particle[:] = [s[i] for i in xrange(len(s) / 2)]
particle_color[:] = [0.5, 0.5, 0.5, 0.5]
particles.append(particle)
particle_joint_colors.append(particle_color)
self.robot.updateViewerValues(joint_values,
joint_velocities,
particles,
particle_joint_colors)
def get_jacobian(robot, state):
s = v_double()
s[:] = [state[i] for i in xrange(len(state))]
ee_jacobian = v2_double()
robot.getEndEffectorJacobian(s, ee_jacobian)
jac = []
for i in xrange(len(ee_jacobian)):
jac_el = [ee_jacobian[i][j] for j in xrange(len(ee_jacobian[i]))]
jac.append(jac_el)
return np.array(jac)
def visualize_x(self, robot, x, color=None):
cjvals = v_double()
cjvels = v_double()
cjvals[:] = [x[i] for i in xrange(len(x) / 2)]
cjvels[:] = [x[i] for i in xrange(len(x) / 2, len(x))]
particle_joint_values = v2_double()
robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_values)
def show_state_and_cov(self, state, cov, samples):
joint_values = v_double()
joint_values[:] = [state[i] for i in xrange(len(state) / 2)]
joint_velocities = v_double()
joint_velocities[:] = [
state[i] for i in xrange(len(state) / 2, len(state))
]
particles = v2_double()
particle_joint_colors = v2_double()
#samples = self.sample_multivariate_normal(state, cov, 50)
for s in samples:
particle = v_double()
particle_color = v_double()
particle[:] = [s[i] for i in xrange(len(s) / 2)]
particle_color[:] = [0.5, 0.5, 0.5, 0.5]
particles.append(particle)
particle_joint_colors.append(particle_color)
self.robot.updateViewerValues(joint_values, joint_velocities,
particles, particle_joint_colors)
def play_states(self, states, col, particles):
if not self.viewer_initialized:
self.robot.setupViewer(self.robot_file, self.environment_file)
self.viewer_initialized = True
for i in xrange(len(states)):
cjvals = v_double()
cjvels = v_double()
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)
self.robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_colors)
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor())
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 show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
if not self.viewer_initialized:
self.robot.setupViewer(self.robot_file, self.environment_file)
self.viewer_initialized = True
self.robot.removePermanentViewerParticles()
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.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def transform_ee_position(robot, position):
"""
Transform position to first joint frame
"""
active_joints = v_string()
robot.getActiveJoints(active_joints)
joint_origins = v2_double()
robot.getJointOrigin(active_joints, joint_origins)
joint_origin_first_joint = [joint_origins[0][i] for i in xrange(3)]
position = [position[i] - joint_origin_first_joint[i] for i in xrange(len(position))]
return np.array(position)
def transform_goal(self, goal_position):
"""
Transform goal position to first joint frame
"""
active_joints = v_string()
self.robot.getActiveJoints(active_joints)
joint_origins = v2_double()
self.robot.getJointOrigin(active_joints, joint_origins)
joint_origin_first_joint = [joint_origins[0][i] for i in xrange(3)]
goal_position = [goal_position[i] - joint_origin_first_joint[i] for i in xrange(len(goal_position))]
return goal_position
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
if not self.viewer_initialized:
self.robot.setupViewer(self.robot_file, self.environment_file)
self.viewer_initialized = True
self.robot.removePermanentViewerParticles()
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.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def get_jacobian(robot, state):
s = v_double()
s[:] = [state[i] for i in xrange(len(state))]
ee_jacobian = v2_double()
robot.getEndEffectorJacobian(s, ee_jacobian)
jac = []
for i in xrange(len(ee_jacobian)):
jac_el = [ee_jacobian[i][j] for j in xrange(len(ee_jacobian[i]))]
jac.append(jac_el)
return np.array(jac)
def visualize_paths(self, robot, paths, colors=None):
robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for k in xrange(len(paths)):
for p in paths[k]:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
if colors == None:
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
else:
c = v_double()
c[:] = color
particle_joint_colors.append(c)
particle_joint_values[:] = pjvs
self.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def visualize_paths(self, robot, paths, colors=None):
robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for k in xrange(len(paths)):
for p in paths[k]:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
if colors == None:
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
else:
c = v_double()
c[:] = color
particle_joint_colors.append(c)
particle_joint_values[:] = pjvs
self.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def update_viewer(self, x, z, control_duration=None, colliding_obstacle=None):
""" Update the viewer to display the state 'x' of the robot
Optionally sleep for 'control_duration'
"""
if self.show_viewer:
cjvals = v_double()
cjvels = v_double()
cjvals_arr = [x[i] for i in xrange(len(x) / 2)]
cjvels_arr = [x[i] for i in xrange(len(x) / 2, len(x))]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
pjv = v_double()
pjv[:] = [z[i] for i in xrange(len(z))]
particle_joint_values.append(pjv)
particle_joint_colors = v2_double()
pja = v_double()
pja[:] = [0.5, 0.5, 0.9, 0.0]
particle_joint_colors.append(pja)
self.robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_colors)
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor())
if colliding_obstacle != None:
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]
self.robot.setObstacleColor(colliding_obstacle.getName(),
diffuse_col,
ambient_col)
time.sleep(control_duration)
time.sleep(control_duration)
def generate(self,
start_state,
goal_position,
goal_threshold,
num_generated_goal_states):
"""
Goal position is w.r.t. base frame
"""
goal_position = self.transform_goal(goal_position)
possible_ik_solutions = ik.get_goal_states(self.robot,
goal_position,
self.obstacles,
num=num_generated_goal_states)
solutions = []
n = 0
logging.warn("IKSolutionGenerator: " + str(len(possible_ik_solutions)) + " possible ik solutions found")
for i in xrange(len(possible_ik_solutions)):
logging.info("IKSolutionGenerator: Checking ik solution " + str(i) + " for validity")
ik_solution = [possible_ik_solutions[i][k] for k in xrange(len(start_state) / 2)]
ik_solution.extend([0.0 for j in xrange(len(start_state) / 2)])
goal_states = v2_double()
goal_state = v_double()
goal_state[:] = ik_solution
goal_states[:] = [goal_state]
goal_position_vec = v_double()
goal_position_vec[:] = goal_position
print "check for " + str([[goal_states[i][j] for j in xrange(len(goal_states[i]))] for i in xrange(len(goal_states))])
self.path_planner.setGoalStates(goal_states, goal_position_vec, goal_threshold)
#self.path_planner.set_start_and_goal(start_state, [ik_solution], goal_position, goal_threshold)
start_state_vec = v_double()
start_state_vec[:] = start_state
path = self.path_planner.solve(start_state_vec, self.path_timeout)
if len(path) != 0:
logging.warn("IKSolutionGenerator: ik solution " + str(i) + " is a valid ik solution")
solutions.append(ik_solution)
else:
logging.warn("IKSolutionGenerator: Path has length 0")
n += 1
self.path_planner = None
if not len(solutions) == 0:
print "IKSolutionGenerator: Found " + str(len(solutions)) + " valid goal states"
return solutions
else:
logging.error("IKSoultionGenerator: Couldn't find a valid IK solution. Defined problem seems to be infeasible.")
self.path_planner = None
return []
def _construct(self, robot, obstacles):
path_planner2 = None
if not self.dynamic_problem:
path_planner2 = libpath_planner.PathPlanner(
robot, self.delta_t, self.continuous_collision,
self.max_velocity, 1.0, self.use_linear_path, self.verbose,
self.planning_algorithm)
path_planner2.setup()
else:
path_planner2 = libdynamic_path_planner.DynamicPathPlanner(
robot, self.verbose)
path_planner2.setupMotionValidator(self.continuous_collision)
logging.info(
"PathPlanningInterface: Set up motion validator. Setting kinematics..."
)
logging.info(
"PathPlanningInterface: Kinematics set. Running setup...")
path_planner2.setup(self.simulation_step_size, self.delta_t)
path_planner2.setControlSampler(self.control_sampler)
num_control_samples = libdynamic_path_planner.v_int()
num_control_samples[:] = [self.num_control_samples]
path_planner2.setNumControlSamples(num_control_samples)
path_planner2.setRRTGoalBias(self.rrt_goal_bias)
min_max_control_duration = libdynamic_path_planner.v_int()
min_max_control_duration[:] = [
self.min_control_duration, self.max_control_duration
]
path_planner2.setMinMaxControlDuration(min_max_control_duration)
path_planner2.addIntermediateStates(self.add_intermediate_states)
logging.info("PathPlanningInterface: Path planner setup")
path_planner2.setObstacles(obstacles)
goal_states = v2_double()
gs = []
'''
Check if the start state is valid
'''
ss_vec = v_double()
ss_vec[:] = self.start_state
if not path_planner2.isValid(ss_vec):
logging.warn("Path planning interface: Start state not valid")
return [], [], [], [], False
'''
Check of the goal states are valid
'''
for i in xrange(len(self.goal_states)):
goal_state = v_double()
goal_state[:] = [
self.goal_states[i][j]
for j in xrange(len(self.goal_states[i]))
]
if path_planner2.isValid(goal_state):
gs.append(goal_state)
if len(gs) == 0:
logging.warn("PathPlanningInterface: No valid goal states")
return [], [], [], [], False
goal_states[:] = gs
ee_goal_position = v_double()
ee_goal_position[:] = self.ee_goal_position
path_planner2.setGoalStates(goal_states, ee_goal_position,
self.ee_goal_threshold)
start_state = v_double()
v = [self.start_state[i] for i in xrange(len(self.start_state))]
start_state[:] = v
logging.info("PathPlanningInterface: Solve...")
xs_temp = path_planner2.solve(start_state, self.path_timeout)
xs = []
us = []
zs = []
control_durations = []
for i in xrange(len(xs_temp)):
xs.append([xs_temp[i][j] for j in xrange(0, 2 * self.robot_dof)])
us.append([
xs_temp[i][j]
for j in xrange(2 * self.robot_dof, 3 * self.robot_dof)
])
zs.append([
xs_temp[i][j]
for j in xrange(3 * self.robot_dof, 5 * self.robot_dof)
])
control_durations.append(xs_temp[i][5 * self.robot_dof])
'''if self.dynamic_problem:
all_states = v2_double()
print "getting all states"
path_planner2.getAllStates(all_states)
print "got all states " + str(len(all_states))
plot_states_states = [np.array(all_states[i][0:3]) for i in xrange(len(all_states))]
plot_states_velocities = [np.array(all_states[i][3:6]) for i in xrange(len(all_states))]
from plot import plot_3d_points
scale = [-np.pi - 0.01, np.pi + 0.01]
scale2 = [-11, 11]
plot_3d_points(np.array(plot_states_states),
scale,
scale,
scale)
plot_3d_points(np.array(plot_states_velocities),
scale2,
scale2,
scale2)'''
return xs, us, zs, control_durations, True
def _construct(self, robot, obstacles):
path_planner2 = None
if not self.dynamic_problem:
path_planner2 = libpath_planner.PathPlanner(robot,
self.delta_t,
self.continuous_collision,
self.max_velocity,
1.0,
self.use_linear_path,
self.verbose,
self.planning_algorithm)
path_planner2.setup()
else:
path_planner2 = libdynamic_path_planner.DynamicPathPlanner(robot,
self.verbose)
path_planner2.setupMotionValidator(self.continuous_collision)
logging.info("PathPlanningInterface: Set up motion validator. Setting kinematics...")
logging.info("PathPlanningInterface: Kinematics set. Running setup...")
planner_str = "RRT"
path_planner2.setup(self.simulation_step_size,
self.delta_t,
planner_str)
path_planner2.setControlSampler(self.control_sampler)
num_control_samples = v_int()
num_control_samples[:] = [self.num_control_samples]
path_planner2.setNumControlSamples(num_control_samples)
path_planner2.setRRTGoalBias(self.rrt_goal_bias)
min_max_control_duration = v_int()
min_max_control_duration[:] = [self.min_control_duration,
self.max_control_duration]
path_planner2.setMinMaxControlDuration(min_max_control_duration)
path_planner2.addIntermediateStates(self.add_intermediate_states)
logging.info("PathPlanningInterface: Path planner setup")
path_planner2.setObstacles(obstacles)
goal_states = v2_double()
gs = []
'''
Check if the start state is valid
'''
ss_vec = v_double()
ss_vec[:] = self.start_state
if not path_planner2.isValid(ss_vec):
print "Path planning interface: Start state not valid!!!"
return [], [], [], [], False
'''
Check of the goal states are valid
'''
for i in xrange(len(self.goal_states)):
goal_state = v_double()
goal_state[:] = [self.goal_states[i][j] for j in xrange(len(self.goal_states[i]))]
if path_planner2.isValid(goal_state):
gs.append(goal_state)
if len(gs) == 0:
print "PathPlanningInterface: ERROR: No valid goal states"
return [], [], [], [], False
goal_states[:] = gs
ee_goal_position = v_double()
ee_goal_position[:] = self.ee_goal_position
path_planner2.setGoalStates(goal_states, ee_goal_position, self.ee_goal_threshold)
start_state = v_double()
v = [self.start_state[i] for i in xrange(len(self.start_state))]
start_state[:] = v
logging.info("PathPlanningInterface: Solve...")
xs_temp = path_planner2.solve(start_state, self.path_timeout)
xs = []
us = []
zs = []
control_durations = []
for i in xrange(len(xs_temp)):
xs.append([xs_temp[i][j] for j in xrange(0, 2 * self.robot_dof)])
us.append([xs_temp[i][j] for j in xrange(2 * self.robot_dof, 3 * self.robot_dof)])
zs.append([xs_temp[i][j] for j in xrange(3 * self.robot_dof, 5 * self.robot_dof)])
control_durations.append(xs_temp[i][5 * self.robot_dof])
return xs, us, zs, control_durations, True
def play_states(self,
states,
col,
col_obstacles,
coll_states,
particles,
first_particle):
if not self.viewer_initialized:
self.robot.setupViewer(self.robot_file, self.environment_file)
self.viewer_initialized = True
for i in xrange(len(states)):
cjvals = v_double()
cjvels = v_double()
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 - first_particle]:
particle = v_double()
particle_color = v_double()
particle_vec = [p[k] for k in xrange(len(p) / 2)]
particle[:] = particle_vec
particle_color[:] = [0.2, 0.8, 0.5, 0.0]
particle_joint_values.append(particle)
particle_joint_colors.append(particle_color)
if not i == len(coll_states) and coll_states[i] != None:
part = v_double()
part[:] = [coll_states[i][k] for k in xrange(len(coll_states[i]))]
particle_color = v_double()
particle_color[:] = [0.0, 0.0, 0.0, 0.0]
particle_joint_values.append(part)
particle_joint_colors.append(particle_color)
self.robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_colors)
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor())
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)'''
if col_obstacles[i] != None:
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:
if o.getName() == col_obstacles[i]:
self.robot.setObstacleColor(o.getName(),
diffuse_col,
ambient_col)
break
except:
pass
if self.user_input:
raw_input("Press Enter to continue...")
else:
time.sleep(0.1)