Ejemplo n.º 1
0
    for simu_param in simu_params:
        ndt = simu_param['ndt']
        name = simu_param['name']
        print(" Running %s Simulation ".center(conf.LINE_WIDTH, '#') % name)
        simu_type = simu_param['type']
        # build the simulator
        if (simu_type == 'exponential'):
            sim = consim.build_exponential_simulator(dt, ndt, robot.model,
                                                     robot.data, K, B, mu,
                                                     anchor_slipping,
                                                     predicted_forces,
                                                     forward_dynamics_methods,
                                                     exp_max_mul, int_max_mul)
        else:
            sim = consim.build_euler_simulator(dt, ndt, robot.model,
                                               robot.data, K, B, mu,
                                               forward_dynamics_methods)

        # add the  contact points
        cpts = []  # list of all contact points
        for cname in conf.contact_frames:
            if not robot.model.existFrame(cname):
                print(("ERROR: Frame", cname, "does not exist"))
            cpts += [
                sim.add_contact_point(cname, robot.model.getFrameId(cname),
                                      unilateral_contacts)
            ]
        print(" %s Contact Points Added ".center(conf.LINE_WIDTH, '-') %
              (len(cpts)))

        # reset simulator
Ejemplo n.º 2
0
def run_simulation_cpp(q0, v0, simu_params, ground_truth):
    ndt = simu_params['ndt']

    try:
        forward_dyn_method = simu_params['forward_dyn_method']
    except:
        #  1: pinocchio.Minverse()
        #  2: pinocchio.aba()
        #  3: Cholesky factorization
        forward_dyn_method = 1

    if (simu_params['use_exp_int']):
        simu_cpp = consim.build_exponential_simulator(
            dt, ndt, robot.model, robot.data, conf.K, conf.B, conf.mu,
            conf.anchor_slipping_method, compute_predicted_forces,
            forward_dyn_method, exp_max_mul, int_max_mul)
    else:
        simu_cpp = consim.build_euler_simulator(dt, ndt, robot.model,
                                                robot.data, conf.K, conf.B,
                                                conf.mu, forward_dyn_method)

    cpts = []
    for cf in conf.contact_frames:
        if not robot.model.existFrame(cf):
            print(("ERROR: Frame", cf, "does not exist"))
        cpts += [
            simu_cpp.add_contact_point(cf, robot.model.getFrameId(cf),
                                       unilateral_contacts)
        ]

    simu_cpp.reset_state(q0, v0, True)

    t = 0.0
    nc = len(conf.contact_frames)
    results = Empty()
    results.q = np.zeros((nq, N_SIMULATION + 1)) * np.nan
    results.v = np.zeros((nv, N_SIMULATION + 1)) * np.nan
    results.u = np.zeros((nv, N_SIMULATION + 1))
    results.f = np.zeros((3, nc, N_SIMULATION + 1))
    results.p = np.zeros((3, nc, N_SIMULATION + 1))
    results.dp = np.zeros((3, nc, N_SIMULATION + 1))
    results.p0 = np.zeros((3, nc, N_SIMULATION + 1))
    results.slipping = np.zeros((nc, N_SIMULATION + 1))
    results.active = np.zeros((nc, N_SIMULATION + 1))

    results.q[:, 0] = np.copy(q0)
    results.v[:, 0] = np.copy(v0)
    for ci, cp in enumerate(cpts):
        results.f[:, ci, 0] = cp.f
        results.p[:, ci, 0] = cp.x
        results.p0[:, ci, 0] = cp.x_anchor
        results.dp[:, ci, 0] = cp.v
        results.slipping[ci, 0] = cp.slipping
        results.active[ci, 0] = cp.active

    try:
        time_start = time.time()
        for i in range(0, N_SIMULATION):
            for d in range(int(dt_ref / dt)):
                results.u[:, i] = tau.copy()
                simu_cpp.step(results.u[:, i])

            results.q[:, i + 1] = simu_cpp.get_q()
            results.v[:, i + 1] = simu_cpp.get_v()

            for ci, cp in enumerate(cpts):
                results.f[:, ci, i + 1] = cp.f
                results.p[:, ci, i + 1] = cp.x
                results.p0[:, ci, i + 1] = cp.x_anchor
                results.dp[:, ci, i + 1] = cp.v
                results.slipping[ci, i + 1] = cp.slipping
                results.active[ci, i + 1] = cp.active

            if (np.any(np.isnan(results.v[:, i + 1]))
                    or norm(results.v[:, i + 1]) > 1e3):
                raise Exception(
                    "Time %.3f Velocities are too large: %.1f. Stop simulation."
                    % (t, norm(results.v[:, i + 1])))

            if i % PRINT_N == 0:
                print("Time %.3f" % (t))
            t += dt_ref
        print("Real-time factor:", t / (time.time() - time_start))
    except Exception as e:
        print(e)

    if conf.use_viewer:
        for i in range(0, N_SIMULATION):
            if (np.any(np.isnan(results.q[:, i]))):
                break
            time_start_viewer = time.time()
            robot.display(results.q[:, i])
            time_passed = time.time() - time_start_viewer
            if (time_passed < dt):
                time.sleep(dt - time_passed)

    for key in simu_params.keys():
        results.__dict__[key] = simu_params[key]

    return results
Ejemplo n.º 3
0
def run_simulation(conf,
                   dt,
                   N,
                   robot,
                   controller,
                   q0,
                   v0,
                   simu_params,
                   ground_truth=None,
                   comp_times=None):
    import consim
    compute_predicted_forces = False
    nq, nv = robot.nq, robot.nv
    ndt = simu_params['ndt']
    #    use_exp_int = simu_params['use_exp_int']
    name = simu_params['name']
    simu_type = simu_params['simulator']
    try:
        forward_dyn_method = simu_params['forward_dyn_method']
    except:
        # forward_dyn_method Options
        #  1: pinocchio.Minverse()
        #  2: pinocchio.aba()
        #  3: Cholesky factorization
        forward_dyn_method = 1
    try:
        #0: explicit, 1: semi_implicit, 2: classic-explicit
        integration_type = simu_params['integration_type']
    except:
        integration_type = 0
    try:
        max_mat_mult = simu_params['max_mat_mult']
    except:
        max_mat_mult = 100
    try:
        use_balancing = simu_params['use_balancing']
    except:
        use_balancing = True
    try:
        slippageContinues = simu_params['assumeSlippageContinues']
    except:
        slippageContinues = False

    if ('exponential' == simu_type):
        simu = consim.build_exponential_simulator(
            dt, ndt, robot.model, robot.data, conf.K, conf.B, conf.mu,
            conf.anchor_slipping_method, compute_predicted_forces,
            forward_dyn_method, integration_type, max_mat_mult, max_mat_mult,
            use_balancing)
        simu.assumeSlippageContinues(slippageContinues)
    elif ('euler' == simu_type):
        simu = consim.build_euler_simulator(dt, ndt, robot.model, robot.data,
                                            conf.K, conf.B, conf.mu,
                                            forward_dyn_method,
                                            integration_type)
    elif ('implicit-euler' == simu_type):
        try:
            use_fin_diff_dyn = simu_params['use_finite_differences_dynamics']
        except:
            use_fin_diff_dyn = False
        try:
            use_fin_diff_nle = simu_params['use_finite_differences_nle']
        except:
            use_fin_diff_nle = False
        try:
            use_current_state_as_initial_guess = simu_params[
                'use_current_state_as_initial_guess']
        except:
            use_current_state_as_initial_guess = False
        try:
            convergence_threshold = simu_params['convergence_threshold']
        except:
            convergence_threshold = 1e-8
        simu = consim.build_implicit_euler_simulator(dt, ndt, robot.model,
                                                     robot.data, conf.K,
                                                     conf.B, conf.mu)
        simu.set_use_finite_differences_dynamics(use_fin_diff_dyn)
        simu.set_use_finite_differences_nle(use_fin_diff_nle)
        simu.set_use_current_state_as_initial_guess(
            use_current_state_as_initial_guess)
        simu.set_convergence_threshold(convergence_threshold)
    elif ('rk4' == simu_type):
        simu = consim.build_rk4_simulator(dt, ndt, robot.model, robot.data,
                                          conf.K, conf.B, conf.mu,
                                          forward_dyn_method)
    else:
        raise Exception("Unknown simulation type: " + simu_type)

    cpts = []
    for cf in conf.contact_frames:
        if not robot.model.existFrame(cf):
            print(("ERROR: Frame", cf, "does not exist"))
        frame_id = robot.model.getFrameId(cf)
        cpts += [
            simu.add_contact_point(cf, frame_id, conf.unilateral_contacts)
        ]

    simu.reset_state(q0, v0, True)

    t = 0.0
    nc = len(conf.contact_frames)
    results = Empty()
    results.q = np.zeros((nq, N + 1)) * np.nan
    results.v = np.zeros((nv, N + 1))
    results.com_pos = np.zeros((3, N + 1)) * np.nan
    results.com_vel = np.zeros((3, N + 1))
    results.u = np.zeros((nv, N + 1))
    results.f = np.zeros((3, nc, N + 1))
    results.f_avg = np.zeros((3, nc, N + 1))
    results.f_avg2 = np.zeros((3, nc, N + 1))
    results.f_prj = np.zeros((3, nc, N + 1))
    results.f_prj2 = np.zeros((3, nc, N + 1))
    results.p = np.zeros((3, nc, N + 1))
    results.dp = np.zeros((3, nc, N + 1))
    results.p0 = np.zeros((3, nc, N + 1))
    results.slipping = np.zeros((nc, N + 1))
    results.active = np.zeros((nc, N + 1))
    if ('exponential' == simu_type):
        results.mat_mult = np.zeros(N + 1)
        results.mat_norm = np.zeros(N + 1)
    if ('implicit-euler' == simu_type):
        results.avg_iter_num = np.zeros(N + 1)
    results.computation_times = {}

    results.q[:, 0] = np.copy(q0)
    results.v[:, 0] = np.copy(v0)
    results.com_pos[:, 0] = robot.com(results.q[:, 0])
    for ci, cp in enumerate(cpts):
        results.f[:, ci, 0] = cp.f
        results.p[:, ci, 0] = cp.x
        results.p0[:, ci, 0] = cp.x_anchor
        results.dp[:, ci, 0] = cp.v
        results.slipping[ci, 0] = cp.slipping
        results.active[ci, 0] = cp.active
#    print('K*p', conf.K[2]*results.p[2,:,0].squeeze())

    try:
        controller.reset(q0, v0, conf.T_pre)
        consim.stop_watch_reset_all()
        time_start = time.time()
        for i in range(0, N):
            #            robot.display(results.q[:,i])
            if (ground_truth):
                # first reset to ensure active contact points are correctly marked because otherwise the second
                # time I reset the state the anchor points could be overwritten
                reset_anchor_points = True
                simu.reset_state(ground_truth.q[:, i], ground_truth.v[:, i],
                                 reset_anchor_points)
                # then reset anchor points
                for ci, cp in enumerate(cpts):
                    cp.resetAnchorPoint(ground_truth.p0[:, ci, i],
                                        bool(ground_truth.slipping[ci, i]))
                # then reset once againt to compute updated contact forces, but without touching anchor points
                reset_anchor_points = False
                simu.reset_state(ground_truth.q[:, i], ground_truth.v[:, i],
                                 reset_anchor_points)

            results.u[:,
                      i] = controller.compute_control(simu.get_q(),
                                                      simu.get_v())
            simu.step(results.u[:, i])
            #            time.sleep(5)

            results.q[:, i + 1] = simu.get_q()
            results.v[:, i + 1] = simu.get_v()
            results.com_pos[:, i + 1] = robot.com(results.q[:, i + 1])
            #            results.com_vel[:,i+1] = robot.com_vel()

            if ('exponential' == simu_type):
                results.mat_mult[i] = simu.getMatrixMultiplications()
                results.mat_norm[i] = simu.getMatrixExpL1Norm()
            elif ('implicit-euler' == simu_type):
                results.avg_iter_num[i] = simu.get_avg_iteration_number()

            for ci, cp in enumerate(cpts):
                results.f[:, ci, i + 1] = cp.f
                results.f_avg[:, ci, i + 1] = cp.f_avg
                results.f_avg2[:, ci, i + 1] = cp.f_avg2
                results.f_prj[:, ci, i + 1] = cp.f_prj
                results.f_prj2[:, ci, i + 1] = cp.f_prj2
                results.p[:, ci, i + 1] = cp.x
                results.p0[:, ci, i + 1] = cp.x_anchor
                results.dp[:, ci, i + 1] = cp.v
                results.slipping[ci, i + 1] = cp.slipping
                results.active[ci, i + 1] = cp.active
#                if(cp.active != results.active[ci,i]):
#                    print("%.3f"%t, cp.name, 'changed contact state to ', cp.active, cp.x)

            if (np.any(np.isnan(results.v[:, i + 1]))
                    or norm(results.v[:, i + 1]) > 1e5):
                raise Exception(
                    "Time %.3f Velocities are too large: %.1f. Stop simulation."
                    % (t, norm(results.v[:, i + 1])))

#            if i % PRINT_N == 0:
#                print("Time %.3f" % (t))
            t += dt

#        print("Real-time factor:", t/(time.time() - time_start))
#        consim.stop_watch_report(3)
        if (comp_times):
            for s_key, s_value in comp_times.items():
                #                print(s)
                results.computation_times[s_value] = Empty()
                try:
                    results.computation_times[
                        s_value].avg = consim.stop_watch_get_average_time(
                            s_key)
                    results.computation_times[
                        s_value].tot = consim.stop_watch_get_total_time(s_key)
                except:
                    results.computation_times[s_value].avg = np.nan
                    results.computation_times[s_value].tot = np.nan
#        for key in results.computation_times.keys():
#            print("%20s: %.1f us"%(key, results.computation_times[key].avg*1e6))

    except Exception as e:
        #        raise e
        print("Exception while running simulation", e)
        if (comp_times):
            for s_key, s in comp_times.items():
                results.computation_times[s] = Empty()
                results.computation_times[s].avg = np.nan
                results.computation_times[s].tot = np.nan

    if conf.use_viewer:
        play_motion(robot, results.q, dt)

    for key in simu_params.keys():
        results.__dict__[key] = simu_params[key]

    return results
Ejemplo n.º 4
0
# loop over simulations
for simu_param in simu_params:
    offset = np.array([0.0, -0.0, 0.0])
    ndt = simu_param['ndt']
    name = simu_param['name']
    print((" Running %s Simulation ".center(conf.LINE_WIDTH, '#') % name))
    simu_type = simu_param['type']
    # build the simulator
    if (simu_type == 'exponential'):
        sim = consim.build_exponential_simulator(dt, ndt, robot.model,
                                                 robot.data, K, B, mu,
                                                 anchor_slipping,
                                                 predicted_forces, exp_max_mul,
                                                 int_max_mul)
    else:
        sim = consim.build_euler_simulator(dt, ndt, robot.model, robot.data, K,
                                           B, mu)

    # trajectory log
    com_pos = np.empty((N_SIMULATION, 3)) * nan
    com_vel = np.empty((N_SIMULATION, 3)) * nan
    com_acc = np.empty((N_SIMULATION, 3)) * nan
    com_pos_ref = np.empty((N_SIMULATION, 3)) * nan
    com_vel_ref = np.empty((N_SIMULATION, 3)) * nan
    com_acc_ref = np.empty((N_SIMULATION, 3)) * nan
    # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
    com_acc_des = np.empty((N_SIMULATION, 3)) * nan
    # ConSim log
    sim_f = np.empty((N_SIMULATION + 1, len(conf.contact_frames), 3)) * nan
    sim_q = np.empty((N_SIMULATION + 1, robot.nq)) * nan
    contact_x = np.empty((N_SIMULATION + 1, len(conf.contact_frames), 3)) * nan
    contact_v = np.empty((N_SIMULATION + 1, len(conf.contact_frames), 3)) * nan
contact_frames = ['root_joint']
nc = len(contact_frames)

reset_anchor_points = True

if __name__ == "__main__":
    # build the point mass model
    urdf_path = os.path.abspath('../../models/urdf/free_flyer.urdf')
    mesh_path = os.path.abspath('../../models')
    robot = RobotWrapper.BuildFromURDF(urdf_path, [mesh_path],
                                       pin.JointModelFreeFlyer())
    print('RobotWrapper Object Created Successfully!')

    simu = consim.build_euler_simulator(dt, ndt, robot.model, robot.data, K, B,
                                        mu, forward_dyn_method,
                                        integration_type)
    print('Explicit Euler simulator created succesfully')
    half_plane = consim.create_half_plane(K, B, mu, plane_angle)
    print('Half Plane created successfully! ')

    simu.add_object(half_plane)
    print('Half Plane added successfully! ')

    # plane height is 0.5 m at x = -0.5 => start point mass at x,y,z = -.5, 0., 0.75

    q0 = np.array([-.5, 0., .75, 0., 0., 0., 1.])
    v0 = np.zeros(6)
    tau0 = np.zeros(6)
    f = np.zeros((3, nc, N + 1))