Commit 4680d270 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

sample admissible com velocities from which to compute contact candidates

parent cf161ecc
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
from math import sqrt
import numpy as np
''' *********************** USER-PARAMETERS *********************** '''
SOLVER_ID = 0; # classic TSID formulation
SOLVER_CP = 1;
SOLVER_TO_INTEGRATE = [SOLVER_ID];
DATA_FILE_NAME = 'data';
TEXT_FILE_NAME = 'results.txt';
SAVE_DATA = True;
''' INITIAL STATE PARAMETERS '''
MAX_TEST_DURATION = 8000;
dt = 1e-3;
model_path = ["/home_local/dev/hpp/install/share"];
urdfFileName = model_path[0] + "/hrp2_14_description/urdf/hrp2_14_reduced.urdf";
freeFlyer = True;
''' CONTROLLER CONFIGURATION '''
ENABLE_CAPTURE_POINT_LIMITS = False;
ENABLE_TORQUE_LIMITS = True;
ENABLE_FORCE_LIMITS = False;
ENABLE_JOINT_LIMITS = True;
IMPOSE_POSITION_BOUNDS = True;
IMPOSE_VELOCITY_BOUNDS = True;
IMPOSE_VIABILITY_BOUNDS = True;
IMPOSE_ACCELERATION_BOUNDS = True;
JOINT_POS_PREVIEW = 1.5; # preview window to convert joint pos limits into joint acc limits
JOINT_VEL_PREVIEW = 1; # preview window to convert joint vel limits into joint acc limits
MAX_JOINT_ACC = 30.0;
MAX_MIN_JOINT_ACC = 10.0;
USE_JOINT_VELOCITY_ESTIMATOR = False;
ACCOUNT_FOR_ROTOR_INERTIAS = True;
# CONTROLLER GAINS
kp_posture = 30; #1.0; # proportional gain of postural task
kd_posture = 2*sqrt(kp_posture);
kp_constr = 100.0; # constraint proportional feedback gain
kd_constr = 2*sqrt(kp_constr); # constraint derivative feedback gain
kp_com = 0;
kd_com = 1.0/dt;
kp_ee = 100.0;
kd_ee = 2*sqrt(kp_ee);
constraint_mask = np.array([True, True, True, True, True, True]).T;
ee_mask = np.array([True, True, True, True, True, True]).T;
# CONTROLLER WEIGTHS
w_com = 1;
w_posture = 1e-3; #1e-5; # weight of postural task
# QP SOLVER PARAMETERS
maxIter = 300; # max number of iterations
maxTime = 0.8; # max computation time for the solver in seconds
verb=0; # verbosity level (0, 1, or 2)
# CONTACT PARAMETERS
mu = np.array([0.3, 0.1]); # force and moment friction coefficient
fMin = 1e-3; # minimum normal force
''' SIMULATOR PARAMETERS '''
FORCE_TORQUE_LIMITS = ENABLE_TORQUE_LIMITS;
FORCE_JOINT_LIMITS = ENABLE_JOINT_LIMITS and IMPOSE_POSITION_BOUNDS;
USE_LCP_SOLVER = False
''' STOPPING CRITERIA THRESHOLDS '''
ZERO_JOINT_VEL_THR = 1e-2;
ZERO_COM_VEL_THR = 1e-3;
ZERO_ANG_MOM_THR = 1e-2;
MAX_COM_VELOCITY = 5;
MAX_CONSTRAINT_ERROR = 0.1;
''' INITIAL STATE PARAMETERS '''
INITIAL_CONFIG_ID = 1;
ENSURE_INITIAL_CAPTURE_POINT_OUT = False;
ENSURE_INITIAL_CAPTURE_POINT_IN = False;
ENSURE_STABILITY = False;
ENSURE_UNSTABILITY = True;
ZERO_INITIAL_VERTICAL_COM_VEL = False;
ZERO_INITIAL_ANGULAR_MOMENTUM = True;
MAX_INITIAL_COM_VEL = 1.0;
INITIAL_CONFIG_FILENAME = '../data/1lLeg_0rLeg_configs';
# 1lLeg_0rLeg_configs 1lLeg_0rLeg_4Larm_configs 1lLeg_0rLeg_3Rarm_configs 1lLeg_0rLeg_3Rarm_4Larm_configs
''' VIEWER PARAMETERS '''
ENABLE_VIEWER = False;
PLAY_MOTION_WHILE_COMPUTING = False;
PLAY_MOTION_AT_THE_END = False;
DT_VIEWER = 10*dt; # timestep used to display motion with viewer
SHOW_VIEWER_FLOOR = 'coplanar' in INITIAL_CONFIG_FILENAME;
''' FIGURE PARAMETERS '''
SAVE_FIGURES = False;
SHOW_FIGURES = False;
SHOW_LEGENDS = True;
LINE_ALPHA = 0.7;
BUTTON_PRESS_TIMEOUT = 100.0;
(dp0
S'q'
p1
(lp2
F0.0
aF0.0
aF0.4467552149459755
aF-0.31679550804158285
aF0.0600634466911748
aF0.813899819827778
aF-0.48332191315991685
aF0.38419964430335796
aF0.09897455544689529
aF0.7701587972349491
aF0.5394952430367294
aF-0.7719749779403142
aF1.612378298790683
aF0.9548276882919147
aF-1.004529030500232
aF1.4583167643303125
aF0.8234172897710079
aF0.17
aF-2.422377642244723
aF0.13839173027420681
aF-0.9751997851423452
aF-0.021507424523705243
aF-0.956846199037482
aF-0.7909165661799799
aF0.16649783680750888
aF-0.523599
aF0.610865
aF-0.9154675298603255
aF1.1037960010648176
aF-0.9898158883440943
aF-0.586550494724067
aF-0.3822522245230739
aF-0.5848094534475085
aF-1.9560381533081506
aF1.8136562639076603
aF-0.4973315903473613
aF0.27031316658643934
asS'contact_points'
p3
(dp4
S'LLEG_JOINT5'
p5
(dp6
S'P'
p7
(lp8
(lp9
F0.09
aF0.05
aF-0.105
aa(lp10
F0.09
aF-0.05
aF-0.105
aa(lp11
F-0.09
aF-0.05
aF-0.105
aa(lp12
F-0.09
aF0.05
aF-0.105
aasS'N'
p13
(lp14
(lp15
F1.3552527156068805e-20
aF-1.3552527156068805e-20
aF1.0
aa(lp16
F1.3552527156068805e-20
aF-1.3552527156068805e-20
aF1.0
aa(lp17
F1.3552527156068805e-20
aF-1.3552527156068805e-20
aF1.0
aa(lp18
F1.3552527156068805e-20
aF-1.3552527156068805e-20
aF1.0
aassS'RLEG_JOINT5'
p19
(dp20
g7
(lp21
(lp22
F0.09
aF0.05
aF-0.105
aa(lp23
F0.09
aF-0.05
aF-0.105
aa(lp24
F-0.09
aF-0.05
aF-0.105
aa(lp25
F-0.09
aF0.05
aF-0.105
aasg13
(lp26
(lp27
F1.0339757656912846e-25
aF1.2924697071141057e-25
aF1.0
aa(lp28
F1.0339757656912846e-25
aF1.2924697071141057e-25
aF1.0
aa(lp29
F1.0339757656912846e-25
aF1.2924697071141057e-25
aF1.0
aa(lp30
F1.0339757656912846e-25
aF1.2924697071141057e-25
aF1.0
aasssg13
(lp31
g15
ag16
ag17
ag18
ag27
ag28
ag29
ag30
asg7
(lp32
g9
ag10
ag11
ag12
ag22
ag23
ag24
ag25
as.
\ No newline at end of file
This diff is collapsed.
#!/bin/bash
gepetto-viewer-server &
hpp-rbprm-server &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-viewer-server'
pkill -f 'hpp-rbprm-server'
#!/bin/bash
gepetto-viewer-server &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-viewer-server'
This diff is collapsed.
This diff is collapsed.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment