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)