sample admissible com velocities from which to compute contact candidates

script/tools/tro_capture_point/conf_hpp.py

+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;

script/tools/tro_capture_point/files

+(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

script/tools/tro_capture_point/run.sh

+#!/bin/bash         
+
+gepetto-viewer-server & 
+hpp-rbprm-server &
+ipython -i --no-confirm-exit ./$1
+
+pkill -f  'gepetto-viewer-server'
+pkill -f  'hpp-rbprm-server'

script/tools/tro_capture_point/rund.sh

+#!/bin/bash         
+
+gepetto-viewer-server & 
+ipython -i --no-confirm-exit ./$1
+
+pkill -f  'gepetto-viewer-server'