...
 
Commits (116)
[submodule "cmake"]
path = cmake
url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
#
# Copyright (c) 2020 CNRS
# Authors: Pierre Fernbach
#
#
# This file is part of multicontact-locomotion-planning
# multicontact-locomotion-planning is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-core is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-core If not, see
# <http://www.gnu.org/licenses/>.
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
SET(PROJECT_NAMESPACE loco-3d)
SET(PROJECT_NAME multicontact-locomotion-planning)
SET(PROJECT_DESCRIPTION "Wholebody motion planner for multicontact locomotion")
SET(PROJECT_URL "https://github.com/loco-3d/multicontact-locomotion-planning")
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/python.cmake)
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
FINDPYTHON()
ADD_REQUIRED_DEPENDENCY("eigenpy >= 2.0")
ADD_REQUIRED_DEPENDENCY("curves >= 0.3")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.2")
ADD_REQUIRED_DEPENDENCY("multicontact-api >= 2.0")
ADD_REQUIRED_DEPENDENCY("hpp-rbprm-corba >= 4.8")
ADD_OPTIONAL_DEPENDENCY("tsid >= 1.2")
ADD_OPTIONAL_DEPENDENCY("momentumopt >= 1.0")
#ADD_OPTIONAL_DEPENDENCY("sl1m >= 1.0") #TODO
INSTALL(DIRECTORY momentumopt_configs
DESTINATION "share/mlp")
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(tests)
This diff is collapsed.
Subproject commit 235f3826fa5a6956e9146f8722222c51d0d11bfa
planner_variables:
heuristic: SoftConstraint # Types: TrustRegion, SoftConstraint, TimeOptimization #
load_kinematics: false
display_motion: false
floor_height: 0.0 # offset between the contact position and the 'floor' surface used to constraint the height
min_rel_height: 0.4 # minimum allowed distance from the robot CoM to the floor
heuristic: TrustRegion # Types: TrustRegion, SoftConstraint, TimeOptimization #
n_act_eefs: 4
time_step: 0.05
time_horizon: 9.5
time_horizon: 0. # duration of the motion, will be overwrited by the value inside the contact sequence
time_range: [0.01, 0.2] # Bounds on the value of dt during time optimization
is_time_horizon_fixed: false # Allow to change the total duration of the motion
external_force: [0.00, 0.00, 0.00]
com_displacement: [0., 0., 0.]
num_com_viapoints: 0
com_viapoints:
max_time_iterations: 10000
max_time_iterations: 1000
max_time_residual_tolerance: 1e-3
min_time_residual_improvement: 1e-5
......@@ -27,8 +35,11 @@ planner_variables:
cop_range_lh: [-0.001, 0.001, -0.001, 0.001]
max_eef_lengths: [0.55, 0.55, 0.55, 0.55]
#max_eef_lengths: [0.6, 0.6, 0.6, 0.6]
w_trq_arm: 1.000
w_trq_leg: 1.000
w_time_penalty: 100. # weight of the cost to reduce the total duration of the motion
w_time: 1000.0 # weight of the cost to keep the defined duration of each phases
w_com: [ 10000, 10000, 1000]
w_amom: [ 0.500, 0.500, 0.500]
w_lmom: [ 0.010, 0.001, 0.001]
......@@ -41,6 +52,7 @@ planner_variables:
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_com_track: [ 0.000, 0.000, 0.000] # weight used to track the CoM position from the Kinematic Sequence
w_amom_track: [ 1.000, 1.000, 1.000]
w_lmom_track: [ 0.100, 0.100, 0.100]
......
planner_variables:
heuristic: SoftConstraint # Types: TrustRegion, SoftConstraint, TimeOptimization #
load_kinematics: false
display_motion: false
floor_height: 0.0 # offset between the contact position and the 'floor' surface used to constraint the height
min_rel_height: 0.4 # minimum allowed distance from the robot CoM to the floor
heuristic: TrustRegion # Types: TrustRegion, SoftConstraint, TimeOptimization #
n_act_eefs: 4
time_step: 0.05
time_horizon: 9.5
time_horizon: 0. # duration of the motion, will be overwrited by the value inside the contact sequence
time_range: [0.01, 0.2] # Bounds on the value of dt during time optimization
is_time_horizon_fixed: false # Allow to change the total duration of the motion
external_force: [0.00, 0.00, 0.00]
com_displacement: [0., 0., 0.]
num_com_viapoints: 0
com_viapoints:
max_time_iterations: 10000
max_time_iterations: 1000
max_time_residual_tolerance: 1e-3
min_time_residual_improvement: 1e-5
......@@ -27,8 +35,11 @@ planner_variables:
cop_range_lh: [-0.001, 0.001, -0.001, 0.001]
#max_eef_lengths: [0.58, 0.58, 0.58, 0.58]
max_eef_lengths: [0.75, 0.75, 0.75, 0.75] # was 0.83
w_trq_arm: 1.000
w_trq_leg: 1.000
w_time_penalty: 100. # weight of the cost to reduce the total duration of the motion
w_time: 1000.0 # weight of the cost to keep the defined duration of each phases
w_com: [ 1000., 1000., 1000.]
w_amom: [ 0.500, 0.500, 0.500]
w_lmom: [ 0.010, 0.001, 0.001]
......@@ -41,6 +52,7 @@ planner_variables:
w_frc_leg: [ 0.1, 0.1, 0.1]
w_dfrc_arm: [ 0.000, 0.000, 0.000]
w_dfrc_leg: [ 0.000, 0.000, 0.000]
w_com_track: [ 0.000, 0.000, 0.000] # weight used to track the CoM position from the Kinematic Sequence
w_amom_track: [ 1.000, 1.000, 1.000]
w_lmom_track: [ 0.100, 0.100, 0.100]
......
planner_variables:
heuristic: SoftConstraint # Types: TrustRegion, SoftConstraint, TimeOptimization #
load_kinematics: false
display_motion: false
floor_height: 0.0 # offset between the contact position and the 'floor' surface used to constraint the height
min_rel_height: 0.4 # minimum allowed distance from the robot CoM to the floor
heuristic: TrustRegion # Types: TrustRegion, SoftConstraint, TimeOptimization #
n_act_eefs: 4
time_step: 0.05
time_horizon: 9.5
time_horizon: 0. # duration of the motion, will be overwrited by the value inside the contact sequence
time_range: [0.01, 0.2] # Bounds on the value of dt during time optimization
is_time_horizon_fixed: false # Allow to change the total duration of the motion
external_force: [0.00, 0.00, 0.00]
com_displacement: [0., 0., 0.]
num_com_viapoints: 0
com_viapoints:
max_time_iterations: 10000
max_time_iterations: 1000
max_time_residual_tolerance: 1e-3
min_time_residual_improvement: 1e-5
......@@ -29,7 +37,8 @@ planner_variables:
#max_eef_lengths: [0.6, 0.6, 0.6, 0.6]
w_trq_arm: 1.000
w_trq_leg: 1.000
#w_com: [ 1000, 1000, 1000]
w_time_penalty: 100. # weight of the cost to reduce the total duration of the motion
w_time: 1000.0 # weight of the cost to keep the defined duration of each phases
w_com: [ 10, 10, 1000]
w_amom: [ 0.500, 0.500, 0.500]
w_lmom: [ 0.010, 0.010, 0.001]
......@@ -43,6 +52,7 @@ planner_variables:
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_com_track: [ 0.000, 0.000, 0.000] # weight used to track the CoM position from the Kinematic Sequence
w_amom_track: [ 1.000, 1.000, 1.000]
w_lmom_track: [ 0.100, 0.100, 0.100]
......
......@@ -30,8 +30,8 @@ planner_variables:
# transform used for each effector to compute the maximal distance
eff_offset_rf: [ 0.00, 0.00, 0.00]
eff_offset_lf: [ 0.00, 0.00, 0.00]
eff_offset_rh: [ 0.0, -0.15, 0.45]
eff_offset_lh: [ 0.0, 0.15, 0.45]
eff_offset_rh: [ 0.00, -0.15, 0.45]
eff_offset_lh: [ 0.00, 0.15, 0.45]
# size of the contact surfaces used to constraint the CoP position
cop_range_rf: [-0.03, 0.03, -0.03, 0.03]
cop_range_lf: [-0.03, 0.03, -0.03, 0.03]
......@@ -40,8 +40,8 @@ planner_variables:
w_trq_arm: 0.000
w_trq_leg: 0.000
w_time_penalty: 100. # weight of the cost to reduce the total duration of the motion
w_time: 1000.0 # weight of the cost to keep the defined duration of each phases
w_time_penalty: 100. # weight of the cost to reduce the total duration of the motion
w_time: 1000.0 # weight of the cost to keep the defined duration of each phases
w_com: [ 10000, 100000, 10000] # weight on the final CoM position
w_amom: [ 0.500, 0.500, 0.500] # weight for the minimization of the angular momentum
w_lmom: [ 0.010, 0.001, 0.001] # weight for the minimization of the linear momentum
......@@ -55,7 +55,7 @@ planner_variables:
w_dfrc_arm: [ 0.000, 0.000, 0.000]
w_dfrc_leg: [ 0.000, 0.000, 0.000]
w_com_track: [ 0.000, 0.000, 0.000] # weight used to track the CoM position from the Kinematic Sequence
w_amom_track: [ 100.000, 100.000, 100.000] # weight used to track the angular momentum value from the Kinematic Sequence
w_amom_track: [ 100.0, 100.0, 100.0] # weight used to track the angular momentum value from the Kinematic Sequence
w_lmom_track: [ 0.010, 0.001, 0.001] # weight used to track the linear momentum value from the Kinematic Sequence
......
planner_variables:
load_kinematics: false
display_motion: false
floor_height: 0.0 # offset between the contact position and the 'floor' surface used to constraint the height
min_rel_height: 0.75 # minimum allowed distance from the robot CoM to the floor
heuristic: TimeOptimization # Types: TrustRegion, SoftConstraint, TimeOptimization #
n_act_eefs: 2 # number of effector used for contact, will be overwrited with value from the Contact sequence
time_step: 0.05 # time step used by the solver, will be overwrited by the value defined inside the planner
time_horizon: 0. # duration of the motion, will be overwrited by the value inside the contact sequence
time_range: [0.01, 0.2] # Bounds on the value of dt during time optimization
is_time_horizon_fixed: false # Allow to change the total duration of the motion
external_force: [0., 0., 0.]
com_displacement: [0., 0., 0.] # will be overwrited with values from the contact sequence
num_com_viapoints: 0
com_viapoints:
max_time_iterations: 1000
max_time_residual_tolerance: 1e-3
min_time_residual_improvement: 1e-5
gravity: 9.81
robot_mass: 90.27
friction_coeff: 0.3
friction_cone: LinearCone # Types: LinearCone, SocCone #
torque_range: [-1000.0, 1000.0]
# Constraint on the distance between the CoM and the active contact :
max_eef_lengths: [0.87, 0.87, 0.9, 0.9]
# transform used for each effector to compute the maximal distance
eff_offset_rf: [ 0.00, 0.00, 0.00]
eff_offset_lf: [ 0.00, 0.00, 0.00]
eff_offset_rh: [ 0.00, -0.15, 0.45]
eff_offset_lh: [ 0.00, 0.15, 0.45]
# size of the contact surfaces used to constraint the CoP position
cop_range_rf: [-0.03, 0.03, -0.03, 0.03]
cop_range_lf: [-0.03, 0.03, -0.03, 0.03]
cop_range_rh: [-0.01, 0.01, -0.01, 0.01]
cop_range_lh: [-0.01, 0.01, -0.01, 0.01]
w_trq_arm: 0.000
w_trq_leg: 0.000
w_time_penalty: 100. # weight of the cost to reduce the total duration of the motion
w_time: 1000.0 # weight of the cost to keep the defined duration of each phases
w_com: [ 10000, 100000, 10000] # weight on the final CoM position
w_amom: [ 0.500, 0.500, 0.500] # weight for the minimization of the angular momentum
w_lmom: [ 0.010, 0.001, 0.001] # weight for the minimization of the linear momentum
w_amomd: [ 0.100, 0.400, 0.100] # weight for the minimization of the angular momentum derivative
w_lmomd: [ 0.015, 0.015, 0.015] # weight for the minimization of the linear momentum derivative
w_amom_final: [ 10.00, 10.00, 10.00] # weight on the final angular momentum value
w_lmom_final: [ 10.00, 10.00, 10.00] # weight on the final linear momentum value
w_com_via: [ 1.000, 1.000, 50000.] # weight on the CoM viapoints positions
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_com_track: [ 0.000, 0.000, 0.000] # weight used to track the CoM position from the Kinematic Sequence
w_amom_track: [ 100.0, 100.0, 100.0] # weight used to track the angular momentum value from the Kinematic Sequence
w_lmom_track: [ 0.010, 0.001, 0.001] # weight used to track the linear momentum value from the Kinematic Sequence
store_data: False
use_default_solver_setting: True
SET(${PROJECT_NAME}_PYTHON_FILES
__init__.py
__main__.py
config.py
loco_planner.py
)
SET(${PROJECT_NAME}_CENTROIDAL_FILES
croc.py
geometric.py
__init__.py
load.py
momentumopt.py
muscod.py
none.py
quasistatic.py
)
SET(${PROJECT_NAME}_CONTACT_FILES
__init__.py
load.py
rbprm.py
sl1m.py
)
SET(${PROJECT_NAME}_EFFECTOR_FILES
bezier_constrained.py
bezier_predef.py
__init__.py
limb_rrt_optimized.py
limb_rrt.py
load.py
smoothed_foot.py
)
SET(${PROJECT_NAME}_WHOLEBODY_FILES
croccodyl.py
__init__.py
load.py
none.py
tsid.py
)
SET(${PROJECT_NAME}_SIMULATOR_FILES
__init__.py
pinocchio_integration.py
)
SET(${PROJECT_NAME}_EXPORT_FILES
blender.py
gazebo.py
__init__.py
npz.py
openHRP.py
sotTalosBalance.py
)
SET(${PROJECT_NAME}_UTILS_FILES
check_path.py
computation_tools.py
cs_tools.py
derivative_filters.py
__init__.py
plot.py
requirements.py
status.py
trajectories.py
util.py
wholebody_result.py
)
SET(${PROJECT_NAME}_VIEWER_FILES
__init__.py
display_tools.py
)
SET(${PROJECT_NAME}_DEMOS_FILES
anymal_circle_oriented.py
anymal_circle.py
anymal_flatGround.py
anymal_modular_palet.py
anymal_platform_random.py
anymal_slalom_debris.py
anymal_slalom.py
common_anymal.py
common_hrp2.py
common_hyq.py
common_talos_fixedUpper.py
common_talos.py
darpa_hyq.py
hrp2_flatGround.py
hrp2_lp_complex.py
hrp2_pushRecovery.py
hyq_slalom_debris.py
__init__.py
talos_bauzil_with_stairs.py
talos_circle_oriented.py
talos_circle.py
talos_flatGround.py
talos_flatGround_quasiStatic.py
talos_lp_complex.py
talos_lp_maze.py
talos_lp_slalom.py
talos_mazeEas_oriented.py
talos_mazeEas.py
talos_moveEffector_flat.py
talos_moveEffector_stairs_m10.py
talos_moveEffector_stairs_m15.py
talos_moveEffector_stairs_p10.py
talos_moveEffector_stairs_p15.py
talos_navBauzil.py
talos_obstaclesFeet.py
talos_plateformes_large.py
talos_plateformes.py
talos_plateformes_quasiStatic.py
talos_platform_random.py
talos_pushRecovery.py
talos_randomMove_flat.py
talos_stairs10.py
talos_stairs10_random.py
talos_table.py
)
# Install python files :
FOREACH(file ${${PROJECT_NAME}_PYTHON_FILES})
PYTHON_INSTALL_ON_SITE("mlp" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_PYTHON_FILES})
FOREACH(file ${${PROJECT_NAME}_CENTROIDAL_FILES})
PYTHON_INSTALL_ON_SITE("mlp/centroidal" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_CENTROIDAL_FILES})
FOREACH(file ${${PROJECT_NAME}_CONTACT_FILES})
PYTHON_INSTALL_ON_SITE("mlp/contact_sequence" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_CONTACT_FILES})
FOREACH(file ${${PROJECT_NAME}_EFFECTOR_FILES})
PYTHON_INSTALL_ON_SITE("mlp/end_effector" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_EFFECTOR_FILES})
FOREACH(file ${${PROJECT_NAME}_WHOLEBODY_FILES})
PYTHON_INSTALL_ON_SITE("mlp/wholebody" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_WHOLEBODY_FILES})
FOREACH(file ${${PROJECT_NAME}_SIMULATOR_FILES})
PYTHON_INSTALL_ON_SITE("mlp/simulator" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_SIMULATOR_FILES})
FOREACH(file ${${PROJECT_NAME}_EXPORT_FILES})
PYTHON_INSTALL_ON_SITE("mlp/export" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_EXPORT_FILES})
FOREACH(file ${${PROJECT_NAME}_UTILS_FILES})
PYTHON_INSTALL_ON_SITE("mlp/utils" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_UTILS_FILES})
FOREACH(file ${${PROJECT_NAME}_VIEWER_FILES})
PYTHON_INSTALL_ON_SITE("mlp/viewer" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_VIEWER_FILES})
FOREACH(file ${${PROJECT_NAME}_DEMOS_FILES})
PYTHON_INSTALL_ON_SITE("mlp/demo_configs" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_DEMOS_FILES})
from .loco_planner import LocoPlanner
from .config import Config
import subprocess
import time
import atexit
import argparse
import os
from mlp import LocoPlanner, Config
from hpp.corbaserver.rbprm.utils import ServerManager
# init argument parser
parser = argparse.ArgumentParser(description="Run a multicontact-locomotion-planning scenario")
parser.add_argument('demo_name', type=str, help="The name of the demo configuration file to load. "
"Must be a valid python file, either inside the PYTHONPATH"
"or inside the mlp.demo_configs folder. ")
parser.add_argument("-n", "--no_viewer", help="Run mlp without visualization.",action="store_true")
args = parser.parse_args()
demo_name = args.demo_name
cfg = Config()
cfg.load_scenario_config(demo_name)
with ServerManager('hpp-rbprm-server'):
if not args.no_viewer:
with ServerManager('gepetto-gui'):
loco_planner = LocoPlanner(cfg)
loco_planner.run()
else:
loco_planner = LocoPlanner(cfg)
loco_planner.run()
import numpy as np
import mlp.config as cfg
import multicontact_api
from multicontact_api import ContactPhase, ContactSequence
from curves import bezier, piecewise
......@@ -7,13 +6,17 @@ from mlp.utils.util import createFullbodyStatesFromCS
from mlp.utils.cs_tools import resetCOMtrajectories
from mlp.utils.requirements import Requirements
multicontact_api.switchToNumpyArray()
import logging
logging.basicConfig(format='[%(name)-12s] %(levelname)-8s: %(message)s')
logger = logging.getLogger("croc")
logger.setLevel(logging.WARNING) #DEBUG, INFO or WARNING
class Inputs(Requirements):
class CentroidalInputsCroc(Requirements):
timings = True
consistentContacts = True
configurationValues = True
class Outputs(Inputs):
class CentroidalOutputsCroc(CentroidalInputsCroc):
COMtrajectories = True
COMvalues = True
......@@ -54,16 +57,16 @@ def setCOMfromCurve(phase, curve_normalized):
# Not generic .... assume that there is always 2 contact phases for each state in fullBody
def generateCentroidalTrajectory(cs, cs_initGuess=None, fb=None, viewer=None, first_iter = True):
def generate_centroidal_croc(cfg, cs, cs_initGuess=None, fb=None, viewer=None, first_iter = True):
if cs_initGuess:
print("WARNING : in centroidal.croc, initial guess is ignored.")
logger.warning("Initial guess is ignored.")
if not fb:
raise ValueError("CROC called without fullBody object.")
if not first_iter:
print("WARNING: in centroidal.croc, it is useless to iterate several times.")
logger.warning("It is useless to iterate several times.")
beginId, endId = createFullbodyStatesFromCS(cs, fb)
print("beginid = ", beginId)
print("endId = ", endId)
logger.info("beginid = %d", beginId)
logger.info("endId = %d", endId)
cs_result = ContactSequence(cs)
resetCOMtrajectories(cs_result) # erase all pre existing CoM trajectories
......@@ -74,12 +77,12 @@ def generateCentroidalTrajectory(cs, cs_initGuess=None, fb=None, viewer=None, fi
id_phase = 0
current_t = cs.contactPhases[0].timeInitial
for id_state in range(beginId, endId):
print("id_state = ", str(id_state))
print("id_phase = ", str(id_phase))
logger.info("id_state = %d", id_state)
logger.info("id_phase = %d", id_phase)
# First, compute the bezier curves between the states :
pid = fb.isDynamicallyReachableFromState(id_state, id_state + 1, True, numPointsPerPhases=0)
if len(pid) != 4:
print("# ERROR : Cannot compute CROC between states " + str(id_state) + " , " + str(id_state + 1))
logger.error("# Cannot compute CROC between states %d, %d", id_state, id_state + 1)
return cs
c1 = fb.getPathAsBezier(int(pid[1])) # curve before contact break
c2 = fb.getPathAsBezier(int(pid[2])) # curve for swing phase
......
import numpy as np
import mlp.config as cfg
import multicontact_api
from multicontact_api import ContactPhase, ContactSequence
from mlp.utils.requirements import Requirements
from mlp.utils.util import genCOMTrajFromPhaseStates, genAMTrajFromPhaseStates
from mlp.utils.cs_tools import computePhasesCOMValues
import math
VERBOSE = False
import logging
logging.basicConfig(format='[%(name)-12s] %(levelname)-8s: %(message)s')
logger = logging.getLogger("geometric")
logger.setLevel(logging.WARNING) #DEBUG, INFO or WARNING
multicontact_api.switchToNumpyArray()
class Inputs(Requirements):
class CentroidalInputsGeometric(Requirements):
timings = True
consistentContacts = True
class Outputs(Inputs):
class CentroidalOutputsGeometric(CentroidalInputsGeometric):
centroidalTrajectories = True
COMvalues = True
def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter = True):
def generate_centroidal_geometric(cfg, cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter = True):
"""
Generate straight line trajectories from the center of the support polygon of one phase
to the center in the next phase.
......@@ -32,9 +34,9 @@ def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=No
:return:
"""
if cs_initGuess:
print("WARNING : in centroidal.geometric, initial guess is ignored.")
logger.warning("Initial guess is ignored.")
if not first_iter:
print("WARNING : in centroidal.geometric, it is useless to iterate several times.")
logger.warning("It is useless to iterate several times.")
if cs.haveCOMvalues():
# do not overwrite com values in input sequence
......
import mlp.config as cfg
import multicontact_api
from multicontact_api import ContactSequence
from mlp.utils.requirements import Requirements as Inputs
from mlp.utils.requirements import Requirements as CentroidalInputsLoad
import logging
logging.basicConfig(format='[%(name)-12s] %(levelname)-8s: %(message)s')
logger = logging.getLogger("load cs")
logger.setLevel(logging.WARNING) #DEBUG, INFO or WARNING
multicontact_api.switchToNumpyArray()
class Outputs(Inputs):
class CentroidalOutputsLoad(CentroidalInputsLoad):
consistentContacts = True
timings = True
centroidalTrajectories = True
centroidalValues = True
def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=None):
def generate_centroidal_load(cfg, cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter = True):
cs_res = ContactSequence(0)
cs_res.loadFromBinary(cfg.COM_FILENAME)
print("Import contact sequence binary file : ", cfg.COM_FILENAME)
logger.warning("Import contact sequence binary file : %s", cfg.COM_FILENAME)
return cs_res
def generate_centroidal_muscod(cfg, cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter = True):
raise NotImplemented("TODO")
from mlp.utils.requirements import Requirements as CentroidalInputsNone
from mlp.utils.requirements import Requirements as CentroidalOutputsNone
def generate_centroidal_none(cfg, cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter = True):
print("Centroidal trajectory not computed !")
import numpy as np
import mlp.config as cfg
import multicontact_api
from multicontact_api import ContactPhase, ContactSequence
from mlp.utils.util import createFullbodyStatesFromCS
from mlp.utils.util import createFullbodyStatesFromCS, perturbateContactNormal
from mlp.utils.cs_tools import connectPhaseTrajToFinalState, setInitialFromFinalValues, copyPhaseInitToFinal
from mlp.utils.requirements import Requirements
import logging
logging.basicConfig(format='[%(name)-12s] %(levelname)-8s: %(message)s')
logger = logging.getLogger("quasistatic")
logger.setLevel(logging.WARNING) #DEBUG, INFO or WARNING
multicontact_api.switchToNumpyArray()
class Inputs(Requirements):
class CentroidalInputsQuasistatic(Requirements):
timings = True
consistentContacts = True
configurationValues = True
class Outputs(Inputs):
class CentroidalOutputsQuasistatic(CentroidalInputsQuasistatic):
COMtrajectories = True
COMvalues = True
......@@ -22,46 +25,59 @@ class Outputs(Inputs):
## The trajectory of the CoM is a quintic spline with initial and final velocity/acceleration constrained to 0
def getTargetCOMPosition(fullBody, id_state):
print("call 2-pac for ids : "+str(id_state)+ " ; "+str(id_state+1))
tab = fullBody.isReachableFromState(id_state, id_state + 1, True, False)
print("tab results : ",tab)
success = tab[0]
def getTargetCOMPosition(fullBody, id_state, com_shift_z):
s_id_init = id_state
s_id_final = id_state+1
success = False
attempts = 10
while not success and attempts > 0:
logger.info("call 2-pac for ids : %d; %d", s_id_init, s_id_final+1)
tab = fullBody.isReachableFromState(s_id_init, s_id_final, True, False)
logger.info("tab results : %s", tab)
success = tab[0]
attempts -= 1
if not success:
# add a small perturbation in the contact normals
logger.info("2-pac failed. Add perturbation and retry")
s_id_init = perturbateContactNormal(fullBody, id_state)
assert s_id_init > 0, "Failed to change contact normals."
s_id_final = perturbateContactNormal(fullBody, id_state+1)
assert s_id_final > 0, "Failed to change contact normals."
assert success, "2-pac failed for state id : " + str(id_state)
if len(tab) == 7:
cBreak = np.array(tab[1:4]).reshape(-1)
cCreate = np.array(tab[4:7]).reshape(-1)
c = (cBreak + cCreate) / 2.
print("shape c : ",c.shape)
logger.debug("shape c : %s", c.shape)
else:
c = np.array(tab[1:4]).reshape(-1)
print("shape c else : ",c.shape)
logger.debug("shape c else : %s", c.shape)
print("c before shift : ", c)
c[2] += cfg.COM_SHIFT_Z
print("c after shift : ", c)
logger.debug("c before shift : %s", c)
c[2] += com_shift_z
logger.info("c : %s", c)
return c
def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter = True):
def generate_centroidal_quasistatic(cfg, cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter = True):
if cs_initGuess:
print("WARNING : in centroidal.quasiStatic, initial guess is ignored.")
logger.warning("Initial guess is ignored.")
if not fullBody:
raise ValueError("quasiStatic called without fullBody object.")
if not first_iter:
print("WARNING : in centroidal.quasiStatic, it is useless to iterate several times.")
logger.warning("It is useless to iterate several times.")
beginId, endId = createFullbodyStatesFromCS(cs, fullBody)
print("beginid = ", beginId)
print("endId = ", endId)
logger.info("beginid = %d", beginId)
logger.info("endId = %d", endId)
cs_result = ContactSequence(cs)
# for each phase in the cs, create a corresponding FullBody State and call 2-PAC,
# then fill the cs struct with a quintic spline connecting the two points found
id_phase = 0
for id_state in range(beginId, endId + 1):
print("id_state = ", str(id_state))
print("id_phase = ", str(id_phase))
logger.debug("id_state = %d", id_state)
logger.debug("id_phase = %d", id_phase)
phase_fixed = cs_result.contactPhases[id_phase] # phase where the CoM move and the contacts are fixed
# set initial state to be the final one of the previous phase :
if id_phase > 1:
......@@ -70,7 +86,7 @@ def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=No
if id_state == endId:
c = phase_fixed.c_final
else:
c = getTargetCOMPosition(fullBody, id_state)
c = getTargetCOMPosition(fullBody, id_state, cfg.COM_SHIFT_Z)
# set 'c' the final position of current phase :
phase_fixed.c_final = c
phase_fixed.dc_final = np.zeros(3)
......
This diff is collapsed.
from multicontact_api import ContactSequence
import mlp.config as cfg
import mlp.viewer.display_tools as display_tools
from mlp.utils.requirements import Requirements
class Outputs(Requirements):
class ContactOutputsLoad(Requirements):
consistentContacts = True
def generateContactSequence():
def generate_contact_sequence_load(cfg):
fb, v = display_tools.initScene(cfg.Robot, cfg.ENV_NAME)
cs = ContactSequence(0)
print("Import contact sequence binary file : ", cfg.CS_FILENAME)
......
from mlp.utils.cs_tools import addPhaseFromConfig, createPhaseFromConfig
import multicontact_api
from multicontact_api import ContactSequence
import mlp.viewer.display_tools as display_tools
from pinocchio import SE3
from numpy import array
from mlp.utils.cs_tools import walk
from talos_rbprm.talos import Robot # change robot here
multicontact_api.switchToNumpyArray()
ENV_NAME = "multicontact/plateforme_surfaces_noscale"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
gui = v.client.gui
sceneName = v.sceneName
cs = ContactSequence(0)
cs_platforms = ContactSequence(0)
cs_platforms.loadFromBinary("talos_plateformes.cs")
p0_platform = cs_platforms.contactPhases[0]
q = [ 0.0,
0.0,
1.02127,
0.0,
0.0,
0.0,
1., # Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Left Leg
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Right Leg
0.0,
0.006761, # Chest
0.40,
0.24,
-0.6,
-1.45,
0.0,
-0.0,
0.,
-0.005, # Left Arm
-0.4,
-0.24,
0.6,
-1.45,
0.0,
0.0,
0.,
-0.005, # Right Arm
0.,
0.
] + [0]*6
q[:2] = [-0.15, 0.25]
addPhaseFromConfig(fb, cs, q, [fb.rLegId, fb.lLegId])
q[:3] = [0.11, 0.25, 1.18127]
p_up = createPhaseFromConfig(fb, q, [fb.rLegId, fb.lLegId])
#make the first step to the platform
cs.moveEffectorToPlacement(fb.rfoot,p_up.contactPatch(fb.rfoot).placement)
cs.moveEffectorToPlacement(fb.lfoot,p_up.contactPatch(fb.lfoot).placement)
# Make the second step to connect the cs_plateform
cs.moveEffectorToPlacement(fb.rfoot,p0_platform.contactPatch(fb.rfoot).placement)
cs.moveEffectorToPlacement(fb.lfoot,p0_platform.contactPatch(fb.lfoot).placement)
# copy the cs_plateform
for phase in cs_platforms.contactPhases[1:]:
cs.append(phase)
## Add final step :
q[0] = 1.14
p_end = createPhaseFromConfig(fb, q, [fb.rLegId, fb.lLegId])
q[:3] = [1.37, 0.25, 1.02127]
p_floor = createPhaseFromConfig(fb, q, [fb.rLegId, fb.lLegId])
#make a step to got to the edge of the platform
cs.moveEffectorToPlacement(fb.rfoot,p_end.contactPatch(fb.rfoot).placement)
cs.moveEffectorToPlacement(fb.lfoot,p_end.contactPatch(fb.lfoot).placement)
#make the last step on the floor
cs.moveEffectorToPlacement(fb.rfoot,p_floor.contactPatch(fb.rfoot).placement)
cs.moveEffectorToPlacement(fb.lfoot,p_floor.contactPatch(fb.lfoot).placement)
display_tools.displaySteppingStones(cs, gui, sceneName, fb)
filename = "talos_platformes_full.cs"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
......@@ -4,7 +4,6 @@ from numpy import array
import multicontact_api
from multicontact_api import ContactSequence
import mlp.viewer.display_tools as display_tools
import mlp.config as cfg
from talos_rbprm.talos import Robot # change robot here
multicontact_api.switchToNumpyArray()
......@@ -43,6 +42,6 @@ display_tools.displaySteppingStones(cs, gui, sceneName, fb)
DEMO_NAME = "talos_stairs10"
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + DEMO_NAME + ".cs"
filename = DEMO_NAME + ".cs"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
......@@ -2,7 +2,6 @@ from mlp.utils.cs_tools import addPhaseFromConfig, setFinalState
import multicontact_api
from multicontact_api import ContactSequence, ContactPatch
import mlp.viewer.display_tools as display_tools
import mlp.config as cfg
from pinocchio import SE3
from mlp.utils.util import rotatePlacement
from talos_rbprm.talos import Robot # change robot here
......@@ -71,6 +70,6 @@ display_tools.displaySteppingStones(cs, gui, sceneName, fb)
DEMO_NAME = "talos_stairsAirbus"
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + DEMO_NAME + ".cs"
filename = DEMO_NAME + ".cs"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
......@@ -2,7 +2,6 @@ from mlp.utils.cs_tools import addPhaseFromConfig, setFinalState
import multicontact_api
from multicontact_api import ContactSequence
import mlp.viewer.display_tools as display_tools
import mlp.config as cfg
from pinocchio import SE3
from mlp.utils.util import rotatePlacement, computeContactNormal
from talos_rbprm.talos import Robot # change robot here
......@@ -76,7 +75,7 @@ state = State(fb, q=q_ref, limbsIncontact=[fb.rLegId, fb.lLegId])
pLHand = [-2.75, -1.235, 1.02]
pRHand = [-2.75, -2.28, 1.02]
"""
from tools.display_tools import *
from hpp.corbaserver.rbprm.tools.display_tools import *
createSphere('s',v)
moveSphere('s',v,pLHand)
"""
......@@ -119,6 +118,6 @@ display_tools.displaySteppingStones(cs, gui, sceneName, fb)
DEMO_NAME = "talos_stairsAirbus"
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + DEMO_NAME + ".cs"
filename = DEMO_NAME + ".cs"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
......@@ -45,4 +45,4 @@ setFinalState(cs,com, q_ref)
assert cs.haveConsistentContacts()
cs.saveAsBinary("talos_flatGround.cs")
cs.saveAsBinary("step_in_place.cs")
......@@ -63,7 +63,7 @@ cs.contactPhases[-2].c_final = c_r
cs.contactPhases[-1].c_init = c_r
setFinalState(cs,com, q_ref)
cs.saveAsBinary("talos_flatGround.cs")
cs.saveAsBinary("step_in_place_quasistatic.cs")
t = 0
for pid, phase in enumerate(cs.contactPhases):
......@@ -99,4 +99,4 @@ assert cs.haveConsistentContacts()
assert cs.haveCentroidalValues()
assert cs.haveCentroidalTrajectories()
cs.saveAsBinary("talos_flatGround_COM.cs")
cs.saveAsBinary("step_in_place_quasistatic_COM.cs")
......@@ -4,7 +4,6 @@ from multicontact_api import ContactSequence
import mlp.viewer.display_tools as display_tools
from numpy import array
from pinocchio import SE3
import mlp.config as cfg
from talos_rbprm.talos import Robot # change robot here
multicontact_api.switchToNumpyArray()
......@@ -36,6 +35,6 @@ display_tools.displaySteppingStones(cs, gui, sceneName, fb)
DEMO_NAME = "talos_flatGround"
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + DEMO_NAME + ".cs"
filename = DEMO_NAME + ".cs"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
......@@ -2,7 +2,6 @@ import pinocchio as pin
from pinocchio import SE3, Quaternion
from pinocchio.utils import *
import inspect
import mlp.config as cfg
import multicontact_api
from multicontact_api import ContactPhase, ContactSequence, ContactPatch
from mlp.utils.util import SE3FromConfig
......@@ -11,12 +10,14 @@ from mlp.utils.requirements import Requirements
from numpy import array
import enum
import importlib
import logging
logging.basicConfig(format='[%(name)-12s] %(levelname)-8s: %(message)s')
logger = logging.getLogger("rbprm")
logger.setLevel(logging.WARNING) #DEBUG, INFO or WARNING
multicontact_api.switchToNumpyArray()
VERBOSE = True
class Outputs(Requirements):
class ContactOutputsRbprm(Requirements):
consistentContacts = True
COMvalues = True
configurationValues = True
......@@ -44,18 +45,29 @@ def contactPlacementFromRBPRMState(fb, stateId, eeName):
def createPhaseFromRBPRMState(fb, stateId, t_init = -1):
q = fb.getConfigAtState(stateId)
limbs_contact = fb.getAllLimbsInContact(stateId)
return createPhaseFromConfig(fb, q, limbs_contact, t_init)
cp = createPhaseFromConfig(fb, q, limbs_contact, t_init)
if fb.cType == "_3_DOF":
# update the contact normal from the data in fullbody
for limbId in limbs_contact:
eeName = fb.dict_limb_joint[limbId]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(stateId, False, limbId)
normal = np.array(n)
print("New normal : ", normal)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal)
placement = cp.contactPatch(eeName).placement
placement.rotation = quat.matrix()
cp.contactPatch(eeName).placement = placement
print("New placement : ", normal)
return cp
def getContactVariationBetweenStates(fb, s1, s2):
# Determine the contact changes which are going to occur :
variations = fb.getContactsVariations(s1, s2)
if VERBOSE:
print("Contact variations : ", variations)
logger.debug("Contact variations : %s", variations)
assert len(variations) <= 1, "Several changes of contacts in adjacent states, not implemented yet !"
variationValue = VariationType.NONE.value
if len(variations) == 0:
if VERBOSE:
print("no variations !")
logger.debug("no variations !")
movingLimb = None
else:
movingLimb = variations[0]
......@@ -65,10 +77,8 @@ def getContactVariationBetweenStates(fb, s1, s2):
variationValue += VariationType.CREATED.value
# if both if are true, it's a repositionning (it is defined by the sum of the enum types)
variationType = VariationType(variationValue)
if VERBOSE:
print("movingLimb = ", movingLimb)
print("variation type :", end ="")
print(variationType.name)
logger.info("movingLimb = %s", movingLimb)
logger.info("variation type : %s", variationType.name)
return fb.dict_limb_joint[movingLimb],variationType
def setPhaseInitialValues(fb, stateId, phase):
......@@ -98,29 +108,31 @@ def setPhaseFinalValues(fb, stateId, phase):
phase.ddc_final = acc
def runRBPRMScript():
def runRBPRMScript(cfg):
#the following script must produce a sequence of configurations in contact (configs)
# with exactly one contact change between each configurations
# It must also initialise a FullBody object name fullBody and optionnaly a Viewer object named V
if hasattr(cfg, 'SCRIPT_ABSOLUTE_PATH'):
scriptName = cfg.SCRIPT_ABSOLUTE_PATH
else:
scriptName = 'scenarios.' + cfg.SCRIPT_PATH + '.' + cfg.DEMO_NAME
print("Run RBPRM script : ", scriptName)
cp = importlib.import_module(scriptName)
if hasattr(cp, 'beginId'):
beginId = cp.beginId
scriptName = cfg.RBPRM_SCRIPT_PATH + "." + cfg.SCRIPT_PATH + '.' + cfg.DEMO_NAME
logger.warning("Run RBPRM script : %s", scriptName)
module = importlib.import_module(scriptName)
cg = module.ContactGenerator()
cg.run()
if hasattr(cg, 'beginId'):
beginId = cg.beginId
else:
beginId = 0
if hasattr(cp, 'endId'):
endId = cp.endId
if hasattr(cg, 'endId'):
endId = cg.endId
else:
endId = len(cp.configs) - 1
return cp.fullBody, cp.v, beginId, endId
endId = len(cg.configs) - 1
return cg.fullbody, cg.v, beginId, endId
def contactSequenceFromRBPRMConfigs(fb, beginId, endId):
print("generate contact sequence from planning : ")
logger.warning("generate contact sequence from planning : ")
n_states = endId - beginId + 1
# There could be either contact break, creation or repositionning between each adjacent states.
# But there should be only contacts break or creation between each adjacent contactPhases
......@@ -131,8 +143,7 @@ def contactSequenceFromRBPRMConfigs(fb, beginId, endId):
# create initial ContactPhase
cs.append(createPhaseFromRBPRMState(fb, beginId))
for stateId in range(beginId + 1, endId + 1): #from the second state to the last one
if VERBOSE:
print("current state id = ", stateId)
logger.info("current state id = %d", stateId)
previous_phase = cs.contactPhases[-1]
eeName, variationType = getContactVariationBetweenStates(fb,stateId-1, stateId)
......@@ -166,8 +177,8 @@ def contactSequenceFromRBPRMConfigs(fb, beginId, endId):
return cs
def generateContactSequence():
fb, viewer, beginId, endId = runRBPRMScript()
def generate_contact_sequence_rbprm(cfg):
fb, viewer, beginId, endId = runRBPRMScript(cfg)
cs = contactSequenceFromRBPRMConfigs(fb, beginId, endId)
return cs, fb, viewer
......@@ -6,7 +6,6 @@ except ImportError:
message +="See https://gepgitlab.laas.fr/loco-3d/sl1m"
raise ImportError(message)
from pinocchio.utils import *
import mlp.config as cfg
import importlib
import multicontact_api
from multicontact_api import ContactSequence
......@@ -14,16 +13,19 @@ from mlp.utils.cs_tools import addPhaseFromConfig, setFinalState
from mlp.viewer.display_tools import initScene, displaySteppingStones
from pinocchio.utils import matrixToRpy
from pinocchio import Quaternion, SE3
from tools.surfaces_from_path import getSurfacesFromGuideContinuous
from hpp.corbaserver.rbprm.tools.surfaces_from_path import getSurfacesFromGuideContinuous
import random
from mlp.utils.requirements import Requirements
import logging
logging.basicConfig(format='[%(name)-12s] %(levelname)-8s: %(message)s')
logger = logging.getLogger("sl1m")
logger.setLevel(logging.WARNING) #DEBUG, INFO or WARNING
multicontact_api.switchToNumpyArray()
class Outputs(Requirements):
class ContactOutputsSl1m(Requirements):
consistentContacts = True
Z_AXIS = np.array([0, 0, 1])
VERBOSE = False
EPS_Z = 0.005 # offset added to feet z position, otherwise there is collisions with the ground
##### MOVE the next methods to a robot-specific file : #####
......@@ -105,7 +107,7 @@ def normal(phase):
if n[2] < 0.:
for i in range(3):
n[i] = -n[i]
# ~ print "normal ", n
logger.debug("normal %s", n)
return n
......@@ -119,12 +121,11 @@ def quatToConfig(quat):
# FIXME : HARDCODED stuff for talos in this method !
def gen_pb(root_init, R, surfaces):
#print "surfaces = ",surfaces
print("number of surfaces : ", len(surfaces))
print("number of rotation matrix for root : ", len(R))
def gen_pb(root_init, R, surfaces, ref_root_height):
logger.debug("surfaces = %s",surfaces)
logger.info("number of surfaces : %d", len(surfaces))
logger.info("number of rotation matrix for root : %d", len(R))
nphases = len(surfaces)
ref_root_height = cfg.IK_REFERENCE_CONFIG[2]
lf_0 = array(root_init[0:3]) + array([0, 0.085, -ref_root_height]) # values for talos !
rf_0 = array(root_init[0:3]) + array([0, -0.085, -ref_root_height]) # values for talos !
#init_floor_height = surfaces[0][0][2][0]
......@@ -132,7 +133,7 @@ def gen_pb(root_init, R, surfaces):
#lf_0[2] = init_floor_height
#rf_0[2] = init_floor_height
p0 = [lf_0, rf_0]
print("p0 used : ", p0)
logger.info("p0 used : %s", p0)
res = {"p0": p0, "c0": None, "nphases": nphases}
#print "number of rotations values : ",len(R)
#print "R= ",R
......@@ -160,73 +161,76 @@ def gen_pb(root_init, R, surfaces):
return res
def solve(tp):
def solve(planner, guide_step_size, guide_max_yaw, max_surface_area, ref_root_height):
from sl1m.fix_sparsity import solveL1
#surfaces_dict = getAllSurfacesDict(tp.afftool)
#surfaces_dict = getAllSurfacesDict(planner.afftool)
success = False
maxIt = 50
it = 0
defaultStep = cfg.GUIDE_STEP_SIZE
defaultStep = guide_step_size
step = defaultStep
variation = 0.4 # FIXME : put it in config file, +- bounds on the step size
pathId = 0
if hasattr(tp, "pathId"):
pathId = tp.pathId
elif hasattr(tp, "pId"):
pathId = tp.pId
if hasattr(planner, "pathId"):
pathId = planner.pathId
elif hasattr(planner, "pId"):
pathId = planner.pId
else:
pathId = tp.ps.numberPaths() - 1
pathId = planner.ps.numberPaths() - 1
while not success and it < maxIt:
if it > 0:
step = defaultStep + random.uniform(-variation, variation)
#configs = getConfigsFromPath (tp.ps, tp.pathId, step)
#getSurfacesFromPath(tp.rbprmBuilder, configs, surfaces_dict, tp.v, True, False)
viewer = tp.v
#configs = getConfigsFromPath (planner.ps, planner.pathId, step)
#getSurfacesFromPath(planner.rbprmBuilder, configs, surfaces_dict, planner.v, True, False)
viewer = planner.v
if not hasattr(viewer, "client"):
viewer = None
R, surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,
tp.ps,
tp.afftool,
R, surfaces = getSurfacesFromGuideContinuous(planner.rbprmBuilder,
planner.ps,
planner.afftool,
pathId,
viewer,
step,
useIntersection=True,
max_yaw=cfg.GUIDE_MAX_YAW)
pb = gen_pb(tp.q_init, R, surfaces)
max_yaw=guide_max_yaw,
max_surface_area=max_surface_area)
pb = gen_pb(planner.q_init, R, surfaces, ref_root_height)
try:
pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, None)
success = True
except:
print("## Planner failed at iter : " + str(it) + " with step length = " + str(step))
logger.warning("## Planner failed at iter : %d with step length = %f", it, step)
it += 1
if not success:
raise RuntimeError("planner always fail.")
return pathId, pb, coms, footpos, allfeetpos, res
def runLPFromGuideScript():
def runLPFromGuideScript(cfg):
#the following script must produce a
if hasattr(cfg, 'SCRIPT_ABSOLUTE_PATH'):
scriptName = cfg.SCRIPT_ABSOLUTE_PATH
else:
scriptName = 'scenarios.' + cfg.SCRIPT_PATH + '.' + cfg.DEMO_NAME
scriptName = cfg