예제 #1
0
파일: lqg.py 프로젝트: hebinbing/LQG
    def show_state_distribution(self, model_file, env_file):
        self.robot.setupViewer(model_file, env_file)
        M = 30.0 * np.identity(2 * self.robot_dof)
        active_joints = v_string()
        self.robot.getActiveJoints(active_joints)
        self.robot_dof = len(active_joints)

        #x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]
        states = []
        for z in xrange(2000):
            u = [70.0, 70.0, 70.0, 0.0, 0.0, 0.0]
            current_state = v_double()
            current_state[:] = x
            control = v_double()
            control[:] = u
            control_error = v_double()
            ce = self.sample_control_error(M)
            #ce = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
            control_error[:] = ce
            x = None
            for k in xrange(1):
                result = v_double()
                self.robot.propagate(current_state, control, control_error,
                                     self.simulation_step_size, self.delta_t,
                                     result)
                x = [result[i] for i in xrange(len(result))]

                current_state[:] = x
                print x
            states.append(np.array(x[3:6]))
            x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]

            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()
            particle_joint_colors = v2_double()
            self.robot.updateViewerValues(cjvals, cjvels,
                                          particle_joint_values,
                                          particle_joint_values)
        from plot import plot_3d_points
        mins = []
        maxs = []

        x_min = min([states[i][0] for i in xrange(len(states))])
        x_max = max([states[i][0] for i in xrange(len(states))])
        y_min = min([states[i][1] for i in xrange(len(states))])
        y_max = max([states[i][1] for i in xrange(len(states))])
        z_min = min([states[i][2] for i in xrange(len(states))])
        z_max = max([states[i][2] for i in xrange(len(states))])

        scale = [-0.2, 0.2]
        plot_3d_points(np.array(states),
                       x_scale=[x_min, x_max],
                       y_scale=[y_min, y_max],
                       z_scale=[z_min, z_max])
        sleep
예제 #2
0
파일: mpc.py 프로젝트: hoergems/LQG_Newt
 def calc_covariance_value(self, robot, error, covariance_matrix, covariance_type='process'):
     active_joints = v_string()
     robot.getActiveJoints(active_joints)        
     if covariance_type == 'process':
         if self.dynamic_problem:            
             torque_limits = v_double()
             robot.getJointTorqueLimits(active_joints, torque_limits)
             torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
             for i in xrange(len(torque_limits)):
                 torque_range = 2.0 * torque_limits[i]
                 covariance_matrix[i, i] = np.square((torque_range / 100.0) * error)
         else:
             for i in xrange(self.robot_dof):
                 covariance_matrix[i, i] = np.square((self.max_velocity / 100.0) * error)
     else:
         lower_position_limits = v_double()
         upper_position_limits = v_double()
         velocity_limits = v_double()            
         robot.getJointLowerPositionLimits(active_joints, lower_position_limits)
         robot.getJointUpperPositionLimits(active_joints, upper_position_limits)
         robot.getJointVelocityLimits(active_joints, velocity_limits)            
         for i in xrange(self.robot_dof):
             position_range = upper_position_limits[i] - lower_position_limits[i]
             covariance_matrix[i, i] = np.square((position_range / 100.0) * error)            
             velocity_range = 2.0 * velocity_limits[i]
             covariance_matrix[i + self.robot_dof, i + self.robot_dof] = np.square((velocity_range / 100.0) * error)          
     return covariance_matrix            
예제 #3
0
 def setup(self,
           robot,
           M, 
           H, 
           W, 
           N, 
           C, 
           D,
           dynamic_problem, 
           enforce_control_constraints):
     self.M = M
     self.H = H
     self.W = W
     self.N = N
     self.C = C
     self.D = D
     self.dynamic_problem = dynamic_problem
     self.control_constraints_enforced = enforce_control_constraints
     
     active_joints = v_string()
     robot.getActiveJoints(active_joints)
     lower_position_constraints = v_double()
     upper_position_constraints = v_double()
     robot.getJointLowerPositionLimits(active_joints, lower_position_constraints)
     robot.getJointUpperPositionLimits(active_joints, upper_position_constraints)
     
     torque_limits = v_double()
     robot.getJointTorqueLimits(active_joints, torque_limits)
     torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
     torque_limits.extend([0.0 for i in xrange(len(active_joints))])
     self.torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
예제 #4
0
def get_random_state(robot):
    active_joints = v_string()    
    robot.getActiveJoints(active_joints)
    joint_lower_limits = v_double()
    joint_upper_limits = v_double()
    robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
    robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
    state = [np.random.uniform(joint_lower_limits[i], joint_upper_limits[i]) for i in xrange(len(active_joints))]
    return np.array(state)    
예제 #5
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)  
예제 #6
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   
예제 #7
0
def check_constraints(robot, state):
    active_joints = v_string()
    robot.getActiveJoints(active_joints)
    joint_lower_limits = v_double()
    joint_upper_limits = v_double()
    robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
    robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
    for i in xrange(len(state)):
        if (state[i] < joint_lower_limits[i] + 0.00000001
                or state[i] > joint_upper_limits[i] - 0.00000001):
            return False
    return True
예제 #8
0
파일: ik.py 프로젝트: hoergems/LQG_Newt
def check_constraints(robot, state):
    active_joints = v_string()    
    robot.getActiveJoints(active_joints)
    joint_lower_limits = v_double()
    joint_upper_limits = v_double()
    robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
    robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
    for i in xrange(len(state)):
        if (state[i] < joint_lower_limits[i] + 0.00000001 or 
            state[i] > joint_upper_limits[i] - 0.00000001):
            return False
    return True
예제 #9
0
    def setup(self, A, B, C, D, H, M, N, V, W, robot, sample_size, obstacles,
              goal_position, goal_radius, show_viewer, model_file, env_file,
              num_cores):
        self.robot = robot
        self.A = A
        self.B = B
        self.C = C
        self.D = D
        self.H = H
        self.M = M
        self.N = N
        self.V = V
        self.W = W
        self.robot_dof = robot.getDOF()
        self.obstacles = obstacles

        active_joints = v_string()
        robot.getActiveJoints(active_joints)
        lower_position_constraints = v_double()
        upper_position_constraints = v_double()
        velocity_constraints = v_double()
        robot.getJointLowerPositionLimits(active_joints,
                                          lower_position_constraints)
        robot.getJointUpperPositionLimits(active_joints,
                                          upper_position_constraints)
        robot.getJointVelocityLimits(active_joints, velocity_constraints)

        self.lower_position_constraints = [
            lower_position_constraints[i]
            for i in xrange(len(lower_position_constraints))
        ]
        self.upper_position_constraints = [
            upper_position_constraints[i]
            for i in xrange(len(upper_position_constraints))
        ]
        self.velocity_constraints = [
            velocity_constraints[i] for i in xrange(len(velocity_constraints))
        ]

        self.constraints_enforced = robot.constraintsEnforced()
        self.sample_size = sample_size
        self.num_cores = num_cores
        #self.num_cores = cpu_count() - 1
        #self.num_cores = 2
        self.goal_position = goal_position
        self.goal_radius = goal_radius
        self.mutex = Lock()
        self.dynamic_problem = False
        self.show_viewer = show_viewer
        self.model_file = model_file
        self.env_file = env_file
예제 #10
0
 def setup(self, A, B, C, D, H, M, N, V, W,               
           robot, 
           sample_size, 
           obstacles,
           goal_position,
           goal_radius,              
           show_viewer,
           model_file,
           env_file,
           num_cores):
     self.robot = robot       
     self.A = A
     self.B = B
     self.C = C
     self.D = D
     self.H = H
     self.M = M
     self.N = N
     self.V = V
     self.W = W
     self.robot_dof = robot.getDOF()
     self.obstacles = obstacles
     
     active_joints = v_string()
     robot.getActiveJoints(active_joints)
     lower_position_constraints = v_double()
     upper_position_constraints = v_double()
     velocity_constraints = v_double()        
     robot.getJointLowerPositionLimits(active_joints, lower_position_constraints)
     robot.getJointUpperPositionLimits(active_joints, upper_position_constraints)
     robot.getJointVelocityLimits(active_joints, velocity_constraints)
     
     self.lower_position_constraints = [lower_position_constraints[i] for i in xrange(len(lower_position_constraints))]
     self.upper_position_constraints = [upper_position_constraints[i] for i in xrange(len(upper_position_constraints))]
     self.velocity_constraints = [velocity_constraints[i] for i in xrange(len(velocity_constraints))]
     
     self.constraints_enforced = robot.constraintsEnforced()
     self.sample_size = sample_size
     self.num_cores = num_cores
     #self.num_cores = cpu_count() - 1
     #self.num_cores = 2 
     self.goal_position = goal_position 
     self.goal_radius = goal_radius
     self.mutex = Lock()        
     self.dynamic_problem = False        
     self.show_viewer = show_viewer
     self.model_file = model_file
     self.env_file = env_file
예제 #11
0
파일: ik.py 프로젝트: hoergems/abt_newt
def get_random_state(robot):
    active_joints = v_string()    
    robot.getActiveJoints(active_joints)
    joint_lower_limits = v_double()
    joint_upper_limits = v_double()
    robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
    robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
    state = [np.random.uniform(joint_lower_limits[i], joint_upper_limits[i]) for i in xrange(len(active_joints))] 
    if len(active_joints) == 6:
        state = [1.91913489548,
                 0.41647636025,
                 -0.641053542305,
                 -0.628739802863,
                 -0.0964141860436,
                 0.336857099326]
    return np.array(state)    
예제 #12
0
def get_random_state(robot):
    active_joints = v_string()
    robot.getActiveJoints(active_joints)
    joint_lower_limits = v_double()
    joint_upper_limits = v_double()
    robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
    robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
    state = [
        np.random.uniform(joint_lower_limits[i], joint_upper_limits[i])
        for i in xrange(len(active_joints))
    ]
    if len(active_joints) == 6:
        state = [
            1.91913489548, 0.41647636025, -0.641053542305, -0.628739802863,
            -0.0964141860436, 0.336857099326
        ]
    return np.array(state)
예제 #13
0
파일: HRF.py 프로젝트: hebinbing/LQG
 def calc_covariance_value(self,
                           robot,
                           error,
                           covariance_matrix,
                           covariance_type='process'):
     active_joints = v_string()
     robot.getActiveJoints(active_joints)
     if covariance_type == 'process':
         if self.dynamic_problem:
             torque_limits = v_double()
             robot.getJointTorqueLimits(active_joints, torque_limits)
             torque_limits = [
                 torque_limits[i] for i in xrange(len(torque_limits))
             ]
             for i in xrange(len(torque_limits)):
                 torque_range = 2.0 * torque_limits[i]
                 covariance_matrix[i, i] = np.square(
                     (torque_range / 100.0) * error)
         else:
             for i in xrange(self.robot_dof):
                 covariance_matrix[i, i] = np.square(
                     (self.max_velocity / 100.0) * error)
     else:
         lower_position_limits = v_double()
         upper_position_limits = v_double()
         velocity_limits = v_double()
         robot.getJointLowerPositionLimits(active_joints,
                                           lower_position_limits)
         robot.getJointUpperPositionLimits(active_joints,
                                           upper_position_limits)
         robot.getJointVelocityLimits(active_joints, velocity_limits)
         for i in xrange(self.robot_dof):
             position_range = upper_position_limits[
                 i] - lower_position_limits[i]
             covariance_matrix[i, i] = np.square(
                 (position_range / 100.0) * error)
             velocity_range = 2.0 * velocity_limits[i]
             covariance_matrix[i + self.robot_dof,
                               i + self.robot_dof] = np.square(
                                   (velocity_range / 100.0) * error)
     return covariance_matrix
예제 #14
0
파일: lqg.py 프로젝트: hoergems/LQG_Newt
 def show_state_distribution(self, model_file, env_file):
     self.robot.setupViewer(model_file, env_file)       
     M = 30.0 * np.identity(2 * self.robot_dof)
     active_joints = v_string()
     self.robot.getActiveJoints(active_joints)
     self.robot_dof = len(active_joints)
     
     #x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]
     states = []
     for z in xrange(2000):
         u = [70.0, 70.0, 70.0, 0.0, 0.0, 0.0]
         current_state = v_double()
         current_state[:] = x
         control = v_double()
         control[:] = u            
         control_error = v_double()
         ce = self.sample_control_error(M)
         #ce = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         control_error[:] = ce
         x = None
         for k in xrange(1):
             result = v_double()
             self.robot.propagate(current_state,
                                  control,
                                  control_error,
                                  self.simulation_step_size,
                                  self.delta_t,
                                  result)                               
             x = [result[i] for i in xrange(len(result))]
             
             current_state[:] = x
             print x
         states.append(np.array(x[3:6]))
         x = [0.0, -np.pi/ 2.0, 0.0, 0.0, 0.0, 0.0]
         
         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()
         particle_joint_colors = v2_double()
         self.robot.updateViewerValues(cjvals, 
                                       cjvels,
                                       particle_joint_values,
                                       particle_joint_values)
     from plot import plot_3d_points
     mins = []
     maxs = []
     
     x_min = min([states[i][0] for i in xrange(len(states))])
     x_max = max([states[i][0] for i in xrange(len(states))])
     y_min = min([states[i][1] for i in xrange(len(states))])
     y_max = max([states[i][1] for i in xrange(len(states))])
     z_min = min([states[i][2] for i in xrange(len(states))])
     z_max = max([states[i][2] for i in xrange(len(states))])
     
     scale = [-0.2, 0.2]
     plot_3d_points(np.array(states), 
                    x_scale = [x_min, x_max], 
                    y_scale = [y_min, y_max], 
                    z_scale=  [z_min, z_max])
     sleep
예제 #15
0
 def setup_problem(self,
                   A,
                   B,
                   C,
                   D,
                   H,
                   V,
                   W,
                   M,
                   N,
                   robot,
                   enforce_control_constraints,
                   obstacles,
                   goal_position,
                   goal_radius,
                   joint_velocity_limit,                      
                   show_viewer,
                   model_file,
                   env_file):
     self.A = A
     self.B = B
     self.C = C
     self.D = D
     self.H = H
     self.V = V
     self.W = W
     self.M = M
     self.N = N
     self.robot = robot
     self.obstacles = obstacles
     self.goal_position = goal_position
     self.goal_radius = goal_radius        
     
     active_joints = v_string()
     robot.getActiveJoints(active_joints)
     lower_position_constraints = v_double()
     upper_position_constraints = v_double()
     robot.getJointLowerPositionLimits(active_joints, lower_position_constraints)
     robot.getJointUpperPositionLimits(active_joints, upper_position_constraints)
     
     self.lower_position_constraints = [lower_position_constraints[i] for i in xrange(len(lower_position_constraints))]
     self.upper_position_constraints = [upper_position_constraints[i] for i in xrange(len(upper_position_constraints))]
     velocity_constraints = v_double()
     robot.getJointVelocityLimits(active_joints, velocity_constraints)
     self.velocity_constraints = [velocity_constraints[i] for i in xrange(len(velocity_constraints))]
     
     self.max_joint_velocities = [joint_velocity_limit for i in xrange(2 * len(active_joints))]
     torque_limits = v_double()
     robot.getJointTorqueLimits(active_joints, torque_limits)
     torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
     torque_limits.extend([0.0 for i in xrange(len(active_joints))])
     self.torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
     
     self.enforce_constraints = robot.constraintsEnforced()
     self.enforce_constrol_constraints = enforce_control_constraints
     self.dynamic_problem = False
     self.stop_when_colliding = False
     self.show_viewer = show_viewer
     active_joints = v_string()
     self.robot.getActiveJoints(active_joints)
     self.robot_dof = self.robot.getDOF()      
     if show_viewer:
         self.robot.setupViewer(model_file, env_file)               
예제 #16
0
    def setup_problem(self, A, B, C, D, H, V, W, M, N, robot,
                      enforce_control_constraints, obstacles, goal_position,
                      goal_radius, joint_velocity_limit, show_viewer,
                      model_file, env_file, knows_collision):
        self.A = A
        self.B = B
        self.C = C
        self.D = D
        self.H = H
        self.V = V
        self.W = W
        self.M = M
        self.N = N
        self.robot = robot
        self.obstacles = obstacles
        self.goal_position = goal_position
        self.goal_radius = goal_radius

        active_joints = v_string()
        robot.getActiveJoints(active_joints)
        lower_position_constraints = v_double()
        upper_position_constraints = v_double()
        robot.getJointLowerPositionLimits(active_joints,
                                          lower_position_constraints)
        robot.getJointUpperPositionLimits(active_joints,
                                          upper_position_constraints)

        self.lower_position_constraints = [
            lower_position_constraints[i]
            for i in xrange(len(lower_position_constraints))
        ]
        self.upper_position_constraints = [
            upper_position_constraints[i]
            for i in xrange(len(upper_position_constraints))
        ]
        velocity_constraints = v_double()
        robot.getJointVelocityLimits(active_joints, velocity_constraints)
        self.velocity_constraints = [
            velocity_constraints[i] for i in xrange(len(velocity_constraints))
        ]

        self.max_joint_velocities = [
            joint_velocity_limit for i in xrange(2 * len(active_joints))
        ]
        torque_limits = v_double()
        robot.getJointTorqueLimits(active_joints, torque_limits)
        torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
        torque_limits.extend([0.0 for i in xrange(len(active_joints))])
        self.torque_limits = [
            torque_limits[i] for i in xrange(len(torque_limits))
        ]

        self.enforce_constraints = robot.constraintsEnforced()
        self.control_constraints_enforced = enforce_control_constraints
        self.dynamic_problem = False
        self.stop_when_colliding = False
        self.show_viewer = show_viewer
        active_joints = v_string()
        self.robot.getActiveJoints(active_joints)
        self.robot_dof = self.robot.getDOF()
        self.knows_collision = knows_collision
        if show_viewer:
            self.robot.setupViewer(model_file, env_file)