Commit 5d5745be authored by ggory15's avatar ggory15
Browse files

modified timeopt

parent 57440ba2
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
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
......@@ -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 #
......
......@@ -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("")
Supports Markdown
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