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
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
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
# 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))
