def is_in_collision(self, previous_state, state): """ Is the given end effector position in collision with an obstacle? """ collidable_obstacles = [o for o in self.obstacles if not o.isTraversable()] joint_angles_goal = v_double() joint_angles_goal[:] = [state[i] for i in xrange(self.robot_dof)] print previous_state print state collision_objects_goal = self.robot.createRobotCollisionObjects(joint_angles_goal) if len(previous_state) > 0: """ Perform continuous collision checking if previous state is provided """ joint_angles_start = v_double() joint_angles_start[:] = [previous_state[i] for i in xrange(self.robot_dof)] collision_objects_start = self.robot.createRobotCollisionObjects(joint_angles_start) for obstacle in collidable_obstacles: for i in xrange(len(collision_objects_start)): if obstacle.inCollisionContinuous([collision_objects_start[i], collision_objects_goal[i]]): print "return true" return True, obstacle return False, None else: for obstacle in collidable_obstacles: if obstacle.inCollisionDiscrete(collision_objects_goal): return True, None return False, None
def is_terminal(self, state): ja = v_double() ja[:] = [state[j] for j in xrange(len(state) / 2)] ee_position_arr = v_double() self.robot.getEndEffectorPosition(ja, ee_position_arr) ee_position = np.array([ee_position_arr[j] for j in xrange(len(ee_position_arr))]) norm = np.linalg.norm(ee_position - self.goal_position) if norm - 0.01 <= self.goal_radius: return True return False
def is_terminal(self, state): ja = v_double() ja[:] = [state[j] for j in xrange(len(state) / 2)] ee_position_arr = v_double() self.robot.getEndEffectorPosition(ja, ee_position_arr) ee_position = np.array( [ee_position_arr[j] for j in xrange(len(ee_position_arr))]) norm = np.linalg.norm(ee_position - self.goal_position) if norm - 0.01 <= self.goal_radius: return True return False
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 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 init_environment(self): self.environment = Environment() self.environment.setupEnvironment(self.environment_file) self.environment.loadRobotFromURDF(self.robot_file) self.robot = self.environment.getRobot() self.robot_dof = self.robot.getDOF() goal_area = v_double() self.environment.getGoalArea(goal_area) self.goal_position = [goal_area[i] for i in xrange(0, 3)] self.goal_radius = goal_area[3] self.environment.showViewer() return True
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)
def is_in_collision(self, previous_state, state): """ Is the given end effector position in collision with an obstacle? """ collidable_obstacles = [ o for o in self.obstacles if not o.isTraversable() ] joint_angles_goal = v_double() joint_angles_goal[:] = [state[i] for i in xrange(self.robot_dof)] print previous_state print state collision_objects_goal = self.robot.createRobotCollisionObjects( joint_angles_goal) if len(previous_state) > 0: """ Perform continuous collision checking if previous state is provided """ joint_angles_start = v_double() joint_angles_start[:] = [ previous_state[i] for i in xrange(self.robot_dof) ] collision_objects_start = self.robot.createRobotCollisionObjects( joint_angles_start) for obstacle in collidable_obstacles: for i in xrange(len(collision_objects_start)): if obstacle.inCollisionContinuous([ collision_objects_start[i], collision_objects_goal[i] ]): print "return true" return True, obstacle return False, None else: for obstacle in collidable_obstacles: if obstacle.inCollisionDiscrete(collision_objects_goal): return True, None return False, None
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 propagate(env, sensor_name): current_state = v_double() control_input = v_double() control_error = v_double() simulation_step_size = 0.0001 duration = 0.003 result = v_double() robot_dof_values = v_double() robot_dof_values_start = v_double() env.getRobotDOFValues(robot_dof_values) print "len robot_dof_values " + str(len(robot_dof_values)) #cs = [0.0 for i in xrange(len(robot_dof_values) * 2)] cv = [0.0 for i in xrange(len(robot_dof_values))] '''cv[0] = 0.3 cv[1] = -0.3 cv[4] = -0.3 cv[5] = 0.3 cv[6] = 0.3 cv[7] = -0.3 cv[8] = 0.3 cv[9] = -0.3 cv[10] = 0.3 cv[11] = -0.3 cv[12] = -1.5 cv[13] = 1.5 cv[14] = -1.5 cv[15] = 1.5 cv[16] = -1.5 cv[17] = 1.5''' cs = [cv[i] for i in xrange(len(cv))] cs.extend([0.0 for i in xrange(len(robot_dof_values))]) #cv[2] = 0.0 current_state[:] = cs control_input[:] = [0.0 for i in xrange(len(robot_dof_values))] control_error[:] = [0.0 for i in xrange(len(robot_dof_values))] robot = env.getRobot() #control_input[1] = 1.0 robot_dof_values[:] = cv robot_dof_values_start[:] = cv env.setRobotDOFValues(robot_dof_values) while True: #print "propagating" robot_dof_values_start[:] = [current_state[i] for i in xrange(len(current_state) / 2)] robot.propagate(current_state, control_input, control_error, simulation_step_size, duration, result) robot_dof_values[:] = [result[i] for i in xrange(len(result) / 2)] t0 = time.time() collides = env.robotCollidesContinuous(robot_dof_values_start,robot_dof_values) t1 = time.time() - t0 if collides.in_collision: if collides.contact_body_name == "front_left_end_effector" or collides.contact_body_name == "front_left_tibia": #control_input[1] = -50.0 body_point = v_double() body_name = "front_left_end_effector" world_normal = v_double() body_point[:] = [0.0, 0.0, 0.0] world_normal[:] = [0.0, 0.0, 1.0] robot.propagate_constraints(current_state, control_input, control_error, simulation_step_size, duration, body_name, body_point, world_normal, result) print [result[i] for i in xrange(len(result))] print "prop"
sensors = v_string() sensors[:] = [sensor_file] env.loadSensors(sensors) env.showViewer() env.getSensorManager() env.loadRobotFromURDF("model/block_model.urdf") #dof_values = v_double() #dof_values[:] = [1.5778636567,-3.28698057487e-06,4.93129297073e-06,-0.028272851672] #env.setRobotDOFValues(dof_values) #time.sleep(100) env.getRobot().setGravityConstant(9.81) #env.transformSensorToSensorLink(sensor_name) env.initOctree(0.1) robot_dof_values = v_double() env.getRobotDOFValues(robot_dof_values) robot_dof_values_arr = [robot_dof_values[i] for i in xrange(len(robot_dof_values))] propagate(env, sensor_name) time.sleep(100) new_robot_dof_values[:] = robot_dof_values_arr env.setRobotDOFValues(new_robot_dof_values) time.sleep(3) robot_trans = v_double() robot_rot = v_double() robot_trans[:] = [0.0, 0.0, 0.0] robot_rot[:] = [0.4, 0.4, 0.4] env.setRobotTransform(robot_trans, robot_rot) time.sleep(3)