Example #1
0
 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"
Example #2
0
 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)
Example #3
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"           
Example #4
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"
Example #5
0
File: HRF.py Project: hebinbing/LQG
 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)
Example #6
0
 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)            
Example #7
0
    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)
Example #8
0
 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)
Example #9
0
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)
Example #10
0
File: HRF.py Project: hoergems/LQG
 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)              
Example #11
0
 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)
Example #12
0
 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)
Example #13
0
 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)
Example #14
0
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)  
Example #15
0
 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   
Example #16
0
 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)
Example #17
0
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)
    
    
Example #18
0
File: HRF.py Project: hebinbing/LQG
    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)
Example #19
0
File: HRF.py Project: hoergems/LQG
 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)        
Example #20
0
 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)
Example #21
0
 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
Example #23
0
 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
Example #24
0
 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)