predicted_forces = False
unilateral_contacts = True
K = 1.e+5 * np.ones([3, 1])
B = 2.4e+2 * np.ones([3, 1])
dt = 2.e-3
dt_ref = 1e-2 # time step of reference motion
forward_dynamics_methods = 1
exp_max_mul = 100
int_max_mul = 100
PRINT_N = int(conf.PRINT_T / dt)
DISPLAY_N = int(conf.DISPLAY_T / dt)
# load robot
# model_path = "../../models/robot_properties_solo"
robot = loadSolo(False) ## load_solo12_pinocchio(model_path)
print(" Solo Loaded Successfully ".center(conf.LINE_WIDTH, '#'))
# now load reference trajectories
refX = np.load('references/' + whichMotion + '_reference_states.npy')
refU = np.load('references/' + whichMotion + '_reference_controls.npy')
feedBack = np.load('references/' + whichMotion + '_feedback.npy')
tau = np.zeros([robot.model.nv, 1])
N_SIMULATION = refU.shape[0]
tt = np.arange(0.0, N_SIMULATION * dt + dt, dt)
for simu_param in simu_params:
ndt = simu_param['ndt']
name = simu_param['name']
# }]
# EULER INTEGRATOR WITH SEMI-IMPLICIT INTEGRATION
#for i in range(i_min, i_max):
# SIMU_PARAMS += [{
# 'name': 'eul-semi%4d'%(2**i),
# 'method_name': 'eul-semi',
# 'simulator': 'euler',
# 'ndt': 2**i,
# 'forward_dyn_method': 3,
# 'integration_type': 1
# }]
if (robot_name == 'solo'):
import conf_solo_cpp as conf
robot = loadSolo(False)
elif (robot_name == 'romeo' or robot_name == 'talos'):
if (robot_name == 'romeo'):
import conf_romeo_cpp as conf
elif (robot_name == 'talos'):
import conf_talos_cpp as conf
robot = RobotWrapper.BuildFromURDF(conf.urdf, [conf.modelPath],
pin.JointModelFreeFlyer())
contact_point = np.ones((3, 4)) * conf.lz
contact_point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
contact_frames = []
for cf in conf.contact_frames:
parentFrameId = robot.model.getFrameId(cf)
parentJointId = robot.model.frames[parentFrameId].parent
import conf_solo as conf
import consim
from example_robot_data.robots_loader import loadSolo
print("Test solo cpp sim started")
dt = 1e-3
ndt = 10
T = 3.0
kp = 10
kd = 0.05
N = int(T / dt)
# se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ], se3.JointModelFreeFlyer())
robot = loadSolo()
robot.data = se3.Data(robot.model)
if not robot.model.check(robot.data):
print("Python data not consistent with model")
sim = consim.build_simple_simulator(dt, ndt, robot.model, robot.data,
conf.K[0, 0], conf.B[0, 0], conf.K[1, 1],
conf.B[1, 1], conf.mu, conf.mu)
cpts = []
for cf in conf.contact_frames:
if not robot.model.existFrame(cf):
print("ERROR: Frame", cf, "does not exist")
cpts += [sim.add_contact_point(robot.model.getFrameId(cf))]
q = conf.q0
PLOT_MAT_MULT_EXPM = 0
PLOT_MAT_NORM_EXPM = 0
ASSUME_A_INVERTIBLE = 0
USE_CONTROLLER = 1
dt = 0.02 # controller time step
T = 0.02
offset = np.array([0.0, -0.0, 0.0])
amp = np.array([0.0, 0.0, 0.05])
frequency = np.array([0.0, .0, 2.0])
N_SIMULATION = int(T / dt) # number of time steps simulated
PRINT_N = int(conf.PRINT_T / dt)
solo = loadSolo()
nq, nv = solo.nq, solo.nv
simu = RobotSimulator(conf, solo, se3.JointModelFreeFlyer())
# simu = RobotSimulator(conf, solo, se3.JointModelFreeFlyer(), 'logStuff') # With logger enabled
simu.assume_A_invertible = ASSUME_A_INVERTIBLE
q0, v0 = np.copy(simu.q), np.copy(simu.v)
# DEBUG
#q0[2] += 1.0
#q0[2] += 2.5e-2
q0[2] -= 6e-6
#v0[2] -= 10e-2
for name in conf.contact_frames:
simu.add_contact(name, conf.contact_normal, conf.K, conf.B, conf.mu)