modified timeopt

config/cfg_momSc_demo02.yaml

-planner_variables:
-
-  heuristic: SoftConstraint  # Types: TrustRegion, SoftConstraint, TimeOptimization #
-  n_act_eefs: 2
-  time_step: 0.1
-  time_horizon: 9.5
-  external_force: [0.00, 0.00, 0.00]
-  com_displacement: [0.20, 1.05, 0.6]
-  num_com_viapoints: 0
-  com_viapoints:
-    via0: [2.50,    0.500, 0.45,  0.00]
-
-  max_time_iterations: 100
-  max_time_residual_tolerance: 1e-3
-  min_time_residual_improvement: 1e-5
-
-  gravity: 9.81
-  robot_mass: 60.00
-  friction_coeff: 1.0
-  friction_cone: LinearCone  # Types: LinearCone, SocCone #
-  torque_range: [-200.0, 200.0]
-  eff_offset_rf: [ 0.00,  0.00,  0.00]
-  eff_offset_lf: [ 0.00,  0.00,  0.00]
-  eff_offset_rh: [ 0.20,  0.00,  0.48]
-  eff_offset_lh: [-0.20,  0.00,  0.48]
-  cop_range_rf: [-0.03,  0.03, -0.08,  0.08]
-  cop_range_lf: [-0.03,  0.03, -0.08,  0.08]
-  cop_range_rh: [-0.03,  0.03, -0.03,  0.03]
-  cop_range_lh: [-0.03,  0.03, -0.03,  0.03]
-  max_eef_lengths: [0.82,  0.82,  0.60,  0.60]
-
-  w_trq_arm:       1.000
-  w_trq_leg:       1.000
-  w_com:           [ 10000,  10000,  10000]
-  w_amom:          [ 0.500,  0.500,  0.500]
-  w_lmom:          [ 0.010,  0.001,  0.001]
-  w_amomd:         [ 0.100,  0.400,  0.100]
-  w_lmomd:         [ 0.015,  0.015,  0.015]
-  w_amom_final:    [ 10.00,  10.00,  10.00]
-  w_lmom_final:    [ 10.00,  10.00,  10.00]
-  w_com_via:       [ 0.000,  0.000,  0.000]
-  w_frc_arm:       [ 0.001,  0.001,  0.001]
-  w_frc_leg:       [ 0.001,  0.001,  0.001]
-  w_dfrc_arm:      [ 0.000,  0.000,  0.000]
-  w_dfrc_leg:      [ 0.000,  0.000,  0.000]
-  w_amom_track:    [ 1.000,  1.000,  1.000]
-  w_lmom_track:    [ 0.100,  0.100,  0.100]
-
-
-  store_data: True
-  use_default_solver_setting: True

config/cfg_momSc_demo03.yaml

-planner_variables:
-
-  heuristic: SoftConstraint  # Types: TrustRegion, SoftConstraint, TimeOptimization #
-  n_act_eefs: 2
-  time_step: 0.1
-  time_horizon: 9.5
-  external_force: [0.00, 0.00, 0.00]
-  com_displacement: [0.20, 1.05, 0.6]
-  num_com_viapoints: 0
-  com_viapoints:
-    via0: [2.50,    0.500, 0.45,  0.00]
-
-  max_time_iterations: 100
-  max_time_residual_tolerance: 1e-3
-  min_time_residual_improvement: 1e-5
-
-  gravity: 9.81
-  robot_mass: 60.00
-  friction_coeff: 1.0
-  friction_cone: LinearCone  # Types: LinearCone, SocCone #
-  torque_range: [-200.0, 200.0]
-  eff_offset_rf: [ 0.00,  0.00,  0.00]
-  eff_offset_lf: [ 0.00,  0.00,  0.00]
-  eff_offset_rh: [ 0.20,  0.00,  0.48]
-  eff_offset_lh: [-0.20,  0.00,  0.48]
-  cop_range_rf: [-0.03,  0.03, -0.08,  0.08]
-  cop_range_lf: [-0.03,  0.03, -0.08,  0.08]
-  cop_range_rh: [-0.03,  0.03, -0.03,  0.03]
-  cop_range_lh: [-0.03,  0.03, -0.03,  0.03]
-  max_eef_lengths: [0.82,  0.82,  0.60,  0.60]
-
-  w_trq_arm:       1.000
-  w_trq_leg:       1.000
-  w_com:           [ 10000,  10000,  10000]
-  w_amom:          [ 0.500,  0.500,  0.500]
-  w_lmom:          [ 0.010,  0.001,  0.001]
-  w_amomd:         [ 0.100,  0.400,  0.100]
-  w_lmomd:         [ 0.015,  0.015,  0.015]
-  w_amom_final:    [ 10.00,  10.00,  10.00]
-  w_lmom_final:    [ 10.00,  10.00,  10.00]
-  w_com_via:       [ 0.000,  0.000,  0.000]
-  w_frc_arm:       [ 0.001,  0.001,  0.001]
-  w_frc_leg:       [ 0.001,  0.001,  0.001]
-  w_dfrc_arm:      [ 0.000,  0.000,  0.000]
-  w_dfrc_leg:      [ 0.000,  0.000,  0.000]
-  w_amom_track:    [ 1.000,  1.000,  1.000]
-  w_lmom_track:    [ 0.100,  0.100,  0.100]
-
-
-  store_data: True
-  use_default_solver_setting: True

config/cfg_timeopt_demo01.yaml

@@ -6,7 +6,7 @@ planner_variables:
 
   heuristic: TimeOptimization  # Types: TrustRegion, SoftConstraint, TimeOptimization #
   n_act_eefs: 2
-  time_step: 0.1
+  time_step: 0.05
   time_horizon: 9.5
   external_force: [0.00, 0.00, 0.00]
   com_displacement: [0.20, 1.05, 0.6]
@@ -41,7 +41,7 @@ planner_variables:
   cop_range_lf: [-0.08,  0.08, -0.03,  0.03]
   cop_range_rh: [-0.03,  0.03, -0.03,  0.03]
   cop_range_lh: [-0.03,  0.03, -0.03,  0.03]
-  max_eef_lengths: [0.82,  0.82,  0.60,  0.60]
+  max_eef_lengths: [0.75,  0.75,  0.60,  0.60]
 
   ####################
   # Dynamics weights #

script/locomote_test.py

@@ -38,7 +38,6 @@ isInit = True
 isFinal = True
 
 for k in range(num_phases):
-    print k, "th"
     contact_phase = cs.contact_phases[k]
     init_guess_provided = (len(contact_phase.state_trajectory) > 0)
 
@@ -145,7 +144,7 @@ tp.setInitialPose(True, np.matrix((-0.086, 0.15,-0.92)).transpose(), rot, timeop
 tp.setInitialPose(True, np.matrix((0.4, 0.3, 0.0)).transpose(), rot, timeopt.EndeffectorID.RH)
 tp.setInitialPose(True, np.matrix((-0.4, 0.3, 0.0)).transpose(), rot, timeopt.EndeffectorID.LH)
 
-tp.setMass(60.0);
+tp.setMass(88.0);
 tp.getMass();
 tp.setFinalCOM(cs.contact_phases[num_phases-1].final_state[0:3])
 
@@ -155,7 +154,7 @@ for i in range(len(LF)):
     tp.setPhase(i + len(RF), timeopt.phase(timeopt.EndeffectorID.LF, LF_time[i][0, 0], LF_time[i][0, 1], LF[i].translation, LF[i].rotation))
 
 import os
-cfg_path=str(os.path.dirname(os.path.abspath(__file__))) + '/../../../../config/' + 'cfg_momSc_demo03.yaml'
+cfg_path=str(os.path.dirname(os.path.abspath(__file__))) + '/../config/' + 'cfg_timeopt_demo01.yaml'
 tp.setConfigurationFile(cfg_path)
 tp.setTimeoptSolver(cfg_path)
 tp.solve()
@@ -200,4 +199,5 @@ for k in range(tp.getTrajectorySize()):
   cs.contact_phases[cnt].state_trajectory.append(np.matrix(x))
 
 cs.saveAsXML("Result.xml", "ContactSequence")
+print "Generated Result.xml"
 print("")