From 6eedd10c29940530d0415b5e46df3d325e4aebc1 Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Fri, 12 Apr 2019 14:06:23 +0200
Subject: [PATCH] [refactor] renaming : hpp-wholebody-motion ->
multicontact-locomotion-planning (mlp)
---
.../{hpp_wholebody_motion => mlp}/__init__.py | 0
.../centroidal/__init__.py | 0
.../centroidal/croc.py | 2 +-
.../centroidal/geometric.py | 2 +-
.../centroidal/topt.py | 6 ++---
.../{hpp_wholebody_motion => mlp}/config.py | 14 +++++------
.../contact_sequence/__init__.py | 0
.../contact_sequence/rbprm.py | 4 ++--
.../demo_configs/__init__.py | 0
.../demo_configs/anymal_flatGround.py | 0
.../demo_configs/anymal_slalom_debris.py | 0
.../demo_configs/common_anymal.py | 0
.../demo_configs/common_hrp2.py | 0
.../demo_configs/common_hyq.py | 0
.../demo_configs/common_talos.py | 0
.../demo_configs/darpa_hyq.py | 0
.../demo_configs/hrp2_flatGround.py | 0
scripts/mlp/demo_configs/hrp2_pushRecovery.py | 19 +++++++++++++++
.../demo_configs/hyq_slalom_debris.py | 0
.../demo_configs/talos_flatGround.py | 0
.../demo_configs/talos_obstaclesFeet.py | 0
.../mlp/demo_configs/talos_pushRecovery.py | 23 +++++++++++++++++++
.../demo_configs/talos_table.py | 0
.../end_effector/__init__.py | 0
.../end_effector/bezier_constrained.py | 8 +++----
.../end_effector/bezier_predef.py | 6 ++---
.../end_effector/limb_rrt.py | 2 +-
.../export/__init__.py | 0
.../export/blender.py | 4 ++--
.../export/gazebo.py | 2 +-
.../export/openHRP.py | 4 ++--
.../utils/__init__.py | 0
.../utils/check_path.py | 2 +-
.../utils/computation_tools.py | 6 ++---
.../utils/derivative_filters.py | 0
.../utils/plot.py | 6 ++---
.../utils/polyBezier.py | 0
.../utils/trajectories.py | 0
.../utils/util.py | 5 +++-
.../utils/wholebody_result.py | 2 +-
.../viewer/__init__.py | 0
.../viewer/display_tools.py | 2 +-
.../wholebody/__init__.py | 0
.../wholebody/tsid_invdyn.py | 20 ++++++++--------
scripts/run_from_cscom.py | 12 +++++-----
scripts/{run_locomote.py => run_mlp.py} | 22 +++++++++---------
46 files changed, 109 insertions(+), 64 deletions(-)
rename scripts/{hpp_wholebody_motion => mlp}/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/centroidal/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/centroidal/croc.py (98%)
rename scripts/{hpp_wholebody_motion => mlp}/centroidal/geometric.py (98%)
rename scripts/{hpp_wholebody_motion => mlp}/centroidal/topt.py (98%)
rename scripts/{hpp_wholebody_motion => mlp}/config.py (87%)
rename scripts/{hpp_wholebody_motion => mlp}/contact_sequence/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/contact_sequence/rbprm.py (99%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/anymal_flatGround.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/anymal_slalom_debris.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/common_anymal.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/common_hrp2.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/common_hyq.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/common_talos.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/darpa_hyq.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/hrp2_flatGround.py (100%)
create mode 100644 scripts/mlp/demo_configs/hrp2_pushRecovery.py
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/hyq_slalom_debris.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/talos_flatGround.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/talos_obstaclesFeet.py (100%)
create mode 100644 scripts/mlp/demo_configs/talos_pushRecovery.py
rename scripts/{hpp_wholebody_motion => mlp}/demo_configs/talos_table.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/end_effector/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/end_effector/bezier_constrained.py (99%)
rename scripts/{hpp_wholebody_motion => mlp}/end_effector/bezier_predef.py (97%)
rename scripts/{hpp_wholebody_motion => mlp}/end_effector/limb_rrt.py (99%)
rename scripts/{hpp_wholebody_motion => mlp}/export/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/export/blender.py (81%)
rename scripts/{hpp_wholebody_motion => mlp}/export/gazebo.py (91%)
rename scripts/{hpp_wholebody_motion => mlp}/export/openHRP.py (98%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/check_path.py (99%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/computation_tools.py (97%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/derivative_filters.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/plot.py (98%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/polyBezier.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/trajectories.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/util.py (98%)
rename scripts/{hpp_wholebody_motion => mlp}/utils/wholebody_result.py (99%)
rename scripts/{hpp_wholebody_motion => mlp}/viewer/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/viewer/display_tools.py (99%)
rename scripts/{hpp_wholebody_motion => mlp}/wholebody/__init__.py (100%)
rename scripts/{hpp_wholebody_motion => mlp}/wholebody/tsid_invdyn.py (97%)
rename scripts/{run_locomote.py => run_mlp.py} (85%)
diff --git a/scripts/hpp_wholebody_motion/__init__.py b/scripts/mlp/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/__init__.py
rename to scripts/mlp/__init__.py
diff --git a/scripts/hpp_wholebody_motion/centroidal/__init__.py b/scripts/mlp/centroidal/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/centroidal/__init__.py
rename to scripts/mlp/centroidal/__init__.py
diff --git a/scripts/hpp_wholebody_motion/centroidal/croc.py b/scripts/mlp/centroidal/croc.py
similarity index 98%
rename from scripts/hpp_wholebody_motion/centroidal/croc.py
rename to scripts/mlp/centroidal/croc.py
index de104aa..d3a2824 100644
--- a/scripts/hpp_wholebody_motion/centroidal/croc.py
+++ b/scripts/mlp/centroidal/croc.py
@@ -1,5 +1,5 @@
import numpy as np
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import locomote
from locomote import WrenchCone,SOC6,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid
from hpp_spline import bezier
diff --git a/scripts/hpp_wholebody_motion/centroidal/geometric.py b/scripts/mlp/centroidal/geometric.py
similarity index 98%
rename from scripts/hpp_wholebody_motion/centroidal/geometric.py
rename to scripts/mlp/centroidal/geometric.py
index 1702d95..041dedf 100644
--- a/scripts/hpp_wholebody_motion/centroidal/geometric.py
+++ b/scripts/mlp/centroidal/geometric.py
@@ -1,5 +1,5 @@
import numpy as np
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import locomote
from locomote import WrenchCone,SOC6,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid
diff --git a/scripts/hpp_wholebody_motion/centroidal/topt.py b/scripts/mlp/centroidal/topt.py
similarity index 98%
rename from scripts/hpp_wholebody_motion/centroidal/topt.py
rename to scripts/mlp/centroidal/topt.py
index e6b1de5..c9cd641 100644
--- a/scripts/hpp_wholebody_motion/centroidal/topt.py
+++ b/scripts/mlp/centroidal/topt.py
@@ -5,9 +5,9 @@ import timeopt
import locomote
from locomote import WrenchCone,SOC6,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid
import time
-import hpp_wholebody_motion.config as cfg
-import hpp_wholebody_motion.viewer.display_tools as display
-from hpp_wholebody_motion.utils.util import *
+import mlp.config as cfg
+import mlp.viewer.display_tools as display
+from mlp.utils.util import *
CONTACT_ANKLE_LEVEL = True # probably only required for hrp2, as the center of the feet is not the center of the flexibility ...
diff --git a/scripts/hpp_wholebody_motion/config.py b/scripts/mlp/config.py
similarity index 87%
rename from scripts/hpp_wholebody_motion/config.py
rename to scripts/mlp/config.py
index d5b0644..2937359 100644
--- a/scripts/hpp_wholebody_motion/config.py
+++ b/scripts/mlp/config.py
@@ -1,7 +1,7 @@
## PATHS settings :
import numpy as np
import os
-PKG_PATH = os.environ['DEVEL_HPP_DIR']+"/src/hpp-wholebody-motion"
+PKG_PATH = os.environ['DEVEL_HPP_DIR']+"/src/multicontact-locomotion-planning"
OUTPUT_DIR = PKG_PATH+"/res"
CONTACT_SEQUENCE_PATH = OUTPUT_DIR + "/contact_sequences"
TIME_OPT_CONFIG_PATH = PKG_PATH +'/timeOpt_configs'
@@ -11,7 +11,7 @@ SAVE_CS = not LOAD_CS and True
SAVE_CS_COM = not LOAD_CS_COM and True
EXPORT_GAZEBO = False
EXPORT_OPENHRP = True
-openHRP_useZMPref = False
+openHRP_useZMPref = True
EXPORT_PATH = OUTPUT_DIR+"/export"
##DISPLAY settings :
@@ -68,14 +68,14 @@ IK_store_contact_forces = True
import importlib
import sys
if len(sys.argv)<2 :
- print "You must call this script with the name of the config file (one of the file contained in hpp_wholebody_motion.demo_config)"
+ print "You must call this script with the name of the config file (one of the file contained in mlp.demo_config)"
print "Available demo files : "
- configs_path = PKG_PATH+"/scripts/hpp_wholebody_motion/demo_configs"
+ configs_path = PKG_PATH+"/scripts/mlp/demo_configs"
demos_list = os.listdir(configs_path)
for f in demos_list:
if f.endswith(".py") and not f.startswith("__") :
print f.rstrip(".py")
- raise IOError("You must call this script with the name of the config file (one of the file contained in hpp_wholebody_motion.demo_config)")
+ raise IOError("You must call this script with the name of the config file (one of the file contained in mlp.demo_config)")
import argparse
parser = argparse.ArgumentParser(description = "todo")
@@ -85,9 +85,9 @@ DEMO_NAME = args.demo_name
print "# Load demo config : ",DEMO_NAME
# Import the module
try :
- demo_cfg = importlib.import_module('hpp_wholebody_motion.demo_configs.'+DEMO_NAME)
+ demo_cfg = importlib.import_module('mlp.demo_configs.'+DEMO_NAME)
except ImportError:
- raise NameError("No demo config file with the given name in hpp_wholebody_motion.demo_config")
+ raise NameError("No demo config file with the given name in mlp.demo_config")
# Determine a list of names to copy to the current name space
names = getattr(demo_cfg, '__all__', [n for n in dir(demo_cfg) if not n.startswith('_')])
# Copy those names into the current name space
diff --git a/scripts/hpp_wholebody_motion/contact_sequence/__init__.py b/scripts/mlp/contact_sequence/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/contact_sequence/__init__.py
rename to scripts/mlp/contact_sequence/__init__.py
diff --git a/scripts/hpp_wholebody_motion/contact_sequence/rbprm.py b/scripts/mlp/contact_sequence/rbprm.py
similarity index 99%
rename from scripts/hpp_wholebody_motion/contact_sequence/rbprm.py
rename to scripts/mlp/contact_sequence/rbprm.py
index 9e6beec..d2b5562 100644
--- a/scripts/hpp_wholebody_motion/contact_sequence/rbprm.py
+++ b/scripts/mlp/contact_sequence/rbprm.py
@@ -2,11 +2,11 @@ import pinocchio as se3
from pinocchio import SE3, Quaternion
from pinocchio.utils import *
import inspect
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import locomote
from locomote import WrenchCone,SOC6,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid
global i_sphere
-from hpp_wholebody_motion.utils.util import quatFromConfig
+from mlp.utils.util import quatFromConfig
def generateContactSequence(fb,configs,beginId,endId):
diff --git a/scripts/hpp_wholebody_motion/demo_configs/__init__.py b/scripts/mlp/demo_configs/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/__init__.py
rename to scripts/mlp/demo_configs/__init__.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/anymal_flatGround.py b/scripts/mlp/demo_configs/anymal_flatGround.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/anymal_flatGround.py
rename to scripts/mlp/demo_configs/anymal_flatGround.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/anymal_slalom_debris.py b/scripts/mlp/demo_configs/anymal_slalom_debris.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/anymal_slalom_debris.py
rename to scripts/mlp/demo_configs/anymal_slalom_debris.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/common_anymal.py b/scripts/mlp/demo_configs/common_anymal.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/common_anymal.py
rename to scripts/mlp/demo_configs/common_anymal.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/common_hrp2.py b/scripts/mlp/demo_configs/common_hrp2.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/common_hrp2.py
rename to scripts/mlp/demo_configs/common_hrp2.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/common_hyq.py b/scripts/mlp/demo_configs/common_hyq.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/common_hyq.py
rename to scripts/mlp/demo_configs/common_hyq.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/common_talos.py b/scripts/mlp/demo_configs/common_talos.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/common_talos.py
rename to scripts/mlp/demo_configs/common_talos.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/darpa_hyq.py b/scripts/mlp/demo_configs/darpa_hyq.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/darpa_hyq.py
rename to scripts/mlp/demo_configs/darpa_hyq.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/hrp2_flatGround.py b/scripts/mlp/demo_configs/hrp2_flatGround.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/hrp2_flatGround.py
rename to scripts/mlp/demo_configs/hrp2_flatGround.py
diff --git a/scripts/mlp/demo_configs/hrp2_pushRecovery.py b/scripts/mlp/demo_configs/hrp2_pushRecovery.py
new file mode 100644
index 0000000..d96ae6d
--- /dev/null
+++ b/scripts/mlp/demo_configs/hrp2_pushRecovery.py
@@ -0,0 +1,19 @@
+TIMEOPT_CONFIG_FILE = "cfg_softConstraints_hrp2.yaml"
+TIMEOPT_CONFIG_FILE = "cfg_softConstraints_hrp2_eff85.yaml"
+
+from common_hrp2 import *
+SCRIPT_PATH = "sandbox"
+
+DURATION_INIT = 1. # Time to init the motion
+DURATION_FINAL = 1.5 # Time to stop the robot
+DURATION_FINAL_SS = 1.
+DURATION_SS =1.4
+DURATION_DS = 0.3
+DURATION_TS = 0.4
+
+COM_SHIFT_Z = 0.
+TIME_SHIFT_COM = 0.
+
+## Settings for end effectors :
+EFF_T_PREDEF = 0.3
+p_max = 0.1
\ No newline at end of file
diff --git a/scripts/hpp_wholebody_motion/demo_configs/hyq_slalom_debris.py b/scripts/mlp/demo_configs/hyq_slalom_debris.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/hyq_slalom_debris.py
rename to scripts/mlp/demo_configs/hyq_slalom_debris.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/talos_flatGround.py b/scripts/mlp/demo_configs/talos_flatGround.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/talos_flatGround.py
rename to scripts/mlp/demo_configs/talos_flatGround.py
diff --git a/scripts/hpp_wholebody_motion/demo_configs/talos_obstaclesFeet.py b/scripts/mlp/demo_configs/talos_obstaclesFeet.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/talos_obstaclesFeet.py
rename to scripts/mlp/demo_configs/talos_obstaclesFeet.py
diff --git a/scripts/mlp/demo_configs/talos_pushRecovery.py b/scripts/mlp/demo_configs/talos_pushRecovery.py
new file mode 100644
index 0000000..a7b5682
--- /dev/null
+++ b/scripts/mlp/demo_configs/talos_pushRecovery.py
@@ -0,0 +1,23 @@
+TIMEOPT_CONFIG_FILE = "cfg_softConstraints_talos_dt001.yaml"
+from common_talos import *
+SCRIPT_PATH = "sandbox"
+
+DURATION_INIT = 0.5 # Time to init the motion
+DURATION_FINAL = 2.0 # Time to stop the robot
+DURATION_FINAL_SS = 0.8
+DURATION_SS =0.8
+DURATION_DS = 0.2
+DURATION_TS = 0.4
+SOLVER_DT = 0.02 # hardcoded in timeOpt_configs files, must match this one !
+
+
+## Settings for end effectors :
+EFF_T_PREDEF = 0.2
+p_max = 0.1
+
+## setting for the IK :
+kp_com = 10000.0 # proportional gain of center of mass task
+w_com = 1.0 # weight of center of mass task
+kp_Eff = 10000.0 # proportional gain ofthe effectors motion task
+w_eff = 2.0 # weight of the effector motion task
+IK_PRINT_N = 50
\ No newline at end of file
diff --git a/scripts/hpp_wholebody_motion/demo_configs/talos_table.py b/scripts/mlp/demo_configs/talos_table.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/demo_configs/talos_table.py
rename to scripts/mlp/demo_configs/talos_table.py
diff --git a/scripts/hpp_wholebody_motion/end_effector/__init__.py b/scripts/mlp/end_effector/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/end_effector/__init__.py
rename to scripts/mlp/end_effector/__init__.py
diff --git a/scripts/hpp_wholebody_motion/end_effector/bezier_constrained.py b/scripts/mlp/end_effector/bezier_constrained.py
similarity index 99%
rename from scripts/hpp_wholebody_motion/end_effector/bezier_constrained.py
rename to scripts/mlp/end_effector/bezier_constrained.py
index 7af032d..46c4e75 100644
--- a/scripts/hpp_wholebody_motion/end_effector/bezier_constrained.py
+++ b/scripts/mlp/end_effector/bezier_constrained.py
@@ -1,7 +1,7 @@
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import time
import os
-from hpp_wholebody_motion.utils.polyBezier import *
+from mlp.utils.polyBezier import *
import pinocchio as se3
from pinocchio import SE3, Quaternion
from pinocchio.utils import *
@@ -14,14 +14,14 @@ from scipy.spatial import ConvexHull
from tools.disp_bezier import *
import hpp_spline
import hpp_bezier_com_traj as bezier_com
-from hpp_wholebody_motion.utils import trajectories
+from mlp.utils import trajectories
import math
from tools.disp_bezier import *
import eigenpy
import quadprog
import bezier_predef
import limb_rrt
-from hpp_wholebody_motion.utils.util import SE3FromConfig,distPointLine
+from mlp.utils.util import SE3FromConfig,distPointLine
eigenpy.switchToNumpyArray()
diff --git a/scripts/hpp_wholebody_motion/end_effector/bezier_predef.py b/scripts/mlp/end_effector/bezier_predef.py
similarity index 97%
rename from scripts/hpp_wholebody_motion/end_effector/bezier_predef.py
rename to scripts/mlp/end_effector/bezier_predef.py
index 0cd817e..129c392 100644
--- a/scripts/hpp_wholebody_motion/end_effector/bezier_predef.py
+++ b/scripts/mlp/end_effector/bezier_predef.py
@@ -1,7 +1,7 @@
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import time
import os
-from hpp_wholebody_motion.utils.polyBezier import *
+from mlp.utils.polyBezier import *
import pinocchio as se3
from pinocchio import SE3
from pinocchio.utils import *
@@ -12,7 +12,7 @@ from tools.disp_bezier import *
import hpp_spline
from hpp_spline import bezier
import hpp_bezier_com_traj as bezier_com
-from hpp_wholebody_motion.utils import trajectories
+from mlp.utils import trajectories
diff --git a/scripts/hpp_wholebody_motion/end_effector/limb_rrt.py b/scripts/mlp/end_effector/limb_rrt.py
similarity index 99%
rename from scripts/hpp_wholebody_motion/end_effector/limb_rrt.py
rename to scripts/mlp/end_effector/limb_rrt.py
index c0f2b37..46d2b4a 100644
--- a/scripts/hpp_wholebody_motion/end_effector/limb_rrt.py
+++ b/scripts/mlp/end_effector/limb_rrt.py
@@ -1,4 +1,4 @@
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import time
import os
import pinocchio as se3
diff --git a/scripts/hpp_wholebody_motion/export/__init__.py b/scripts/mlp/export/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/export/__init__.py
rename to scripts/mlp/export/__init__.py
diff --git a/scripts/hpp_wholebody_motion/export/blender.py b/scripts/mlp/export/blender.py
similarity index 81%
rename from scripts/hpp_wholebody_motion/export/blender.py
rename to scripts/mlp/export/blender.py
index ce0d9d1..0d11462 100644
--- a/scripts/hpp_wholebody_motion/export/blender.py
+++ b/scripts/mlp/export/blender.py
@@ -1,6 +1,6 @@
import os
-import hpp_wholebody_motion.config as cfg
-import hpp_wholebody_motion.viewer.display_tools as display_tools
+import mlp.config as cfg
+import mlp.viewer.display_tools as display_tools
def export(q_t,v):
path = cfg.EXPORT_PATH+"/blender"
diff --git a/scripts/hpp_wholebody_motion/export/gazebo.py b/scripts/mlp/export/gazebo.py
similarity index 91%
rename from scripts/hpp_wholebody_motion/export/gazebo.py
rename to scripts/mlp/export/gazebo.py
index 416a603..13f1969 100644
--- a/scripts/hpp_wholebody_motion/export/gazebo.py
+++ b/scripts/mlp/export/gazebo.py
@@ -1,5 +1,5 @@
import os
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
def export(q_t):
path = cfg.EXPORT_PATH+"/gazebo"
diff --git a/scripts/hpp_wholebody_motion/export/openHRP.py b/scripts/mlp/export/openHRP.py
similarity index 98%
rename from scripts/hpp_wholebody_motion/export/openHRP.py
rename to scripts/mlp/export/openHRP.py
index c362043..9e34951 100644
--- a/scripts/hpp_wholebody_motion/export/openHRP.py
+++ b/scripts/mlp/export/openHRP.py
@@ -2,14 +2,14 @@
## ONLY WORK FOR HRP-2 !!
import os
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import pinocchio as se3
from pinocchio import SE3, rnea
from pinocchio.utils import *
import numpy as np
from rospkg import RosPack
from pinocchio.robot_wrapper import RobotWrapper
-from hpp_wholebody_motion.utils.computation_tools import *
+from mlp.utils.computation_tools import *
class Struct():
None
diff --git a/scripts/hpp_wholebody_motion/utils/__init__.py b/scripts/mlp/utils/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/utils/__init__.py
rename to scripts/mlp/utils/__init__.py
diff --git a/scripts/hpp_wholebody_motion/utils/check_path.py b/scripts/mlp/utils/check_path.py
similarity index 99%
rename from scripts/hpp_wholebody_motion/utils/check_path.py
rename to scripts/mlp/utils/check_path.py
index 96add52..ad87d58 100644
--- a/scripts/hpp_wholebody_motion/utils/check_path.py
+++ b/scripts/mlp/utils/check_path.py
@@ -1,7 +1,7 @@
# Copyright 2018, LAAS-CNRS
# Author: Pierre Fernbach
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import numpy as np
class PathChecker():
diff --git a/scripts/hpp_wholebody_motion/utils/computation_tools.py b/scripts/mlp/utils/computation_tools.py
similarity index 97%
rename from scripts/hpp_wholebody_motion/utils/computation_tools.py
rename to scripts/mlp/utils/computation_tools.py
index f75e1bd..82a5c9a 100644
--- a/scripts/hpp_wholebody_motion/utils/computation_tools.py
+++ b/scripts/mlp/utils/computation_tools.py
@@ -2,9 +2,9 @@
import numpy as np
import pinocchio as se3
from pinocchio import SE3, Motion,Force
-from hpp_wholebody_motion.utils.util import *
-import hpp_wholebody_motion.config as cfg
-import hpp_wholebody_motion.utils.trajectories as Trajectories
+from mlp.utils.util import *
+import mlp.config as cfg
+import mlp.utils.trajectories as Trajectories
from rospkg import RosPack
from pinocchio.robot_wrapper import RobotWrapper
diff --git a/scripts/hpp_wholebody_motion/utils/derivative_filters.py b/scripts/mlp/utils/derivative_filters.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/utils/derivative_filters.py
rename to scripts/mlp/utils/derivative_filters.py
diff --git a/scripts/hpp_wholebody_motion/utils/plot.py b/scripts/mlp/utils/plot.py
similarity index 98%
rename from scripts/hpp_wholebody_motion/utils/plot.py
rename to scripts/mlp/utils/plot.py
index cf5a77e..1402c8f 100644
--- a/scripts/hpp_wholebody_motion/utils/plot.py
+++ b/scripts/mlp/utils/plot.py
@@ -3,13 +3,13 @@ import numpy as np
import os
import pinocchio as se3
from pinocchio import SE3
-from hpp_wholebody_motion.utils.util import *
-import hpp_wholebody_motion.config as cfg
+from mlp.utils.util import *
+import mlp.config as cfg
import matplotlib
matplotlib.use("Qt4agg")
import matplotlib.pyplot as plt
from multiprocessing import Process
-from hpp_wholebody_motion.utils.computation_tools import computeZMP,computeZMPRef
+from mlp.utils.computation_tools import computeZMP,computeZMPRef
plt.ioff()
diff --git a/scripts/hpp_wholebody_motion/utils/polyBezier.py b/scripts/mlp/utils/polyBezier.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/utils/polyBezier.py
rename to scripts/mlp/utils/polyBezier.py
diff --git a/scripts/hpp_wholebody_motion/utils/trajectories.py b/scripts/mlp/utils/trajectories.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/utils/trajectories.py
rename to scripts/mlp/utils/trajectories.py
diff --git a/scripts/hpp_wholebody_motion/utils/util.py b/scripts/mlp/utils/util.py
similarity index 98%
rename from scripts/hpp_wholebody_motion/utils/util.py
rename to scripts/mlp/utils/util.py
index fb8b144..bd861a6 100644
--- a/scripts/hpp_wholebody_motion/utils/util.py
+++ b/scripts/mlp/utils/util.py
@@ -3,7 +3,7 @@ import numpy as np
from numpy import cross
from numpy.linalg import norm
from pinocchio import SE3, Quaternion
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
def quatFromConfig(q):
return Quaternion(q[6],q[3],q[4],q[5])
@@ -43,6 +43,8 @@ def SE3toVec(M):
v[j + 9] = M.rotation[j, 2]
return v
+# TODO : def SE3FromVec(s):
+
def MotiontoVec(M):
v = np.matrix(np.zeros((6, 1)))
for j in range(3):
@@ -50,6 +52,7 @@ def MotiontoVec(M):
v[j + 3] = M.angular[j]
return v
+
# assume that q.size >= 7 with root pos and quaternion(x,y,z,w)
def SE3FromConfig(q):
placement = SE3.Identity()
diff --git a/scripts/hpp_wholebody_motion/utils/wholebody_result.py b/scripts/mlp/utils/wholebody_result.py
similarity index 99%
rename from scripts/hpp_wholebody_motion/utils/wholebody_result.py
rename to scripts/mlp/utils/wholebody_result.py
index f25e2e5..88c04e1 100644
--- a/scripts/hpp_wholebody_motion/utils/wholebody_result.py
+++ b/scripts/mlp/utils/wholebody_result.py
@@ -1,5 +1,5 @@
import numpy as np
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
class Result:
nq = cfg.nq
diff --git a/scripts/hpp_wholebody_motion/viewer/__init__.py b/scripts/mlp/viewer/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/viewer/__init__.py
rename to scripts/mlp/viewer/__init__.py
diff --git a/scripts/hpp_wholebody_motion/viewer/display_tools.py b/scripts/mlp/viewer/display_tools.py
similarity index 99%
rename from scripts/hpp_wholebody_motion/viewer/display_tools.py
rename to scripts/mlp/viewer/display_tools.py
index 37bdbee..14b1836 100644
--- a/scripts/hpp_wholebody_motion/viewer/display_tools.py
+++ b/scripts/mlp/viewer/display_tools.py
@@ -1,4 +1,4 @@
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import pinocchio as se3
from pinocchio import SE3, Quaternion
import locomote
diff --git a/scripts/hpp_wholebody_motion/wholebody/__init__.py b/scripts/mlp/wholebody/__init__.py
similarity index 100%
rename from scripts/hpp_wholebody_motion/wholebody/__init__.py
rename to scripts/mlp/wholebody/__init__.py
diff --git a/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py b/scripts/mlp/wholebody/tsid_invdyn.py
similarity index 97%
rename from scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py
rename to scripts/mlp/wholebody/tsid_invdyn.py
index f892d47..631b6af 100644
--- a/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py
+++ b/scripts/mlp/wholebody/tsid_invdyn.py
@@ -8,19 +8,19 @@ from rospkg import RosPack
import time
import commands
import gepetto.corbaserver
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import locomote
from locomote import WrenchCone,SOC6,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid
-from hpp_wholebody_motion.utils import trajectories
-import hpp_wholebody_motion.end_effector.bezier_predef as EETraj
-import hpp_wholebody_motion.viewer.display_tools as display_tools
+from mlp.utils import trajectories
+import mlp.end_effector.bezier_predef as EETraj
+import mlp.viewer.display_tools as display_tools
import math
-from hpp_wholebody_motion.utils.wholebody_result import Result
-from hpp_wholebody_motion.utils.util import *
+from mlp.utils.wholebody_result import Result
+from mlp.utils.util import *
if cfg.USE_LIMB_RRT:
- import hpp_wholebody_motion.end_effector.limb_rrt as limb_rrt
+ import mlp.end_effector.limb_rrt as limb_rrt
if cfg.USE_CONSTRAINED_BEZIER:
- import hpp_wholebody_motion.end_effector.bezier_constrained as bezier_constrained
+ import mlp.end_effector.bezier_constrained as bezier_constrained
def createContactForEffector(invdyn,robot,phase,eeName):
@@ -143,7 +143,7 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
data = invdyn.data()
if cfg.EFF_CHECK_COLLISION : # initialise object needed to check the motion
- from hpp_wholebody_motion.utils import check_path
+ from mlp.utils import check_path
validator = check_path.PathChecker(viewer,fullBody,cs,len(q),cfg.WB_VERBOSE)
if cfg.WB_VERBOSE:
@@ -478,7 +478,7 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
if cfg.PLOT:
- from hpp_wholebody_motion.utils import plot
+ from mlp.utils import plot
plot.plotEffectorRef(stored_effectors_ref)
assert (k_t == res.N-1) and "res struct not fully filled."
diff --git a/scripts/run_from_cscom.py b/scripts/run_from_cscom.py
index 16eaae0..b90ba4e 100644
--- a/scripts/run_from_cscom.py
+++ b/scripts/run_from_cscom.py
@@ -1,14 +1,14 @@
-import hpp_wholebody_motion.viewer.display_tools as display_tools
from locomote import ContactSequenceHumanoid
cs_com = ContactSequenceHumanoid(0)
-filename = "/local/dev_hpp/src/hpp-wholebody-motion/res/contact_sequences/talos_table_COM.xml" # A CHANGER
+filename = "/local/dev_hpp/src/hpp-wholebody-motion/res/contact_sequences/talos_table_COM.xml" # FIXME : load it from somewhere
cs_com.loadFromXML(filename, "ContactSequence")
-import hpp_wholebody_motion.wholebody.tsid_invdyn as wb
-q_t = wb.generateWholeBodyMotion(cs_com)
+import mlp.wholebody.tsid_invdyn as wb
+res,robot = wb.generateWholeBodyMotion(cs_com)
-# initialiser un viewer et y charger les models ici ...
+# Init viewer and load models here ...
# TODO
-# display_tools.displayWBmotion(v,q_t,cfg.IK_dt,cfg.DT_DISPLAY)
\ No newline at end of file
+# import mlp.viewer.display_tools as display_tools
+# display_tools.displayWBmotion(v,res.q_t,cfg.IK_dt,cfg.DT_DISPLAY)
\ No newline at end of file
diff --git a/scripts/run_locomote.py b/scripts/run_mlp.py
similarity index 85%
rename from scripts/run_locomote.py
rename to scripts/run_mlp.py
index ccadb75..ed0a7c7 100644
--- a/scripts/run_locomote.py
+++ b/scripts/run_mlp.py
@@ -1,11 +1,11 @@
-import hpp_wholebody_motion.config as cfg
+import mlp.config as cfg
import importlib
#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
cp = importlib.import_module('scenarios.'+cfg.SCRIPT_PATH+'.'+cfg.DEMO_NAME)
-import hpp_wholebody_motion.contact_sequence.rbprm as generate_cs
-import hpp_wholebody_motion.viewer.display_tools as display_tools
+import mlp.contact_sequence.rbprm as generate_cs
+import mlp.viewer.display_tools as display_tools
from locomote import ContactSequenceHumanoid
v = cp.v
@@ -34,11 +34,11 @@ if cfg.DISPLAY_CS_STONES :
if cfg.USE_GEOM_INIT_GUESS:
print "Generate geometric init guess."
- import hpp_wholebody_motion.centroidal.geometric as initGuess_geom
+ import mlp.centroidal.geometric as initGuess_geom
cs_initGuess = initGuess_geom.generateCentroidalTrajectory(cs)
if cfg.USE_CROC_INIT_GUESS:
print "Generate init guess with CROC."
- import hpp_wholebody_motion.centroidal.croc as initGuess_croc
+ import mlp.centroidal.croc as initGuess_croc
cs_initGuess = initGuess_croc.generateCentroidalTrajectory(cs,cp.fullBody,beginState,endState)
if cfg.DISPLAY_INIT_GUESS_TRAJ and (cfg.USE_GEOM_INIT_GUESS or cfg.USE_CROC_INIT_GUESS):
colors = [v.color.red, v.color.yellow]
@@ -49,7 +49,7 @@ if cfg.LOAD_CS_COM :
filename = cfg.CONTACT_SEQUENCE_PATH + "/"+cfg.DEMO_NAME+"_COM.xml"
cs_com.loadFromXML(filename, "ContactSequence")
else:
- import hpp_wholebody_motion.centroidal.topt as centroidal
+ import mlp.centroidal.topt as centroidal
cs_com,tp = centroidal.generateCentroidalTrajectory(cs,cs_initGuess,v)
print "Duration of the motion : "+str(cs_com.contact_phases[-1].time_trajectory[-1])+" s."
@@ -62,7 +62,7 @@ if cfg.DISPLAY_COM_TRAJ:
colors = [v.color.blue, v.color.green]
display_tools.displayCOMTrajectory(cs_com,v,colors)
-import hpp_wholebody_motion.wholebody.tsid_invdyn as wb
+import mlp.wholebody.tsid_invdyn as wb
if cfg.USE_CROC_COM:
assert cfg.USE_CROC_INIT_GUESS, "You must generate CROC initial guess if you want to use it as reference for the COM"
res,robot = wb.generateWholeBodyMotion(cs_initGuess,v,cp.fullBody)
@@ -75,7 +75,7 @@ if cfg.DISPLAY_WB_MOTION:
display_tools.displayWBmotion(v,res.q_t,cfg.IK_dt,cfg.DT_DISPLAY)
if cfg.CHECK_FINAL_MOTION :
- from hpp_wholebody_motion.utils import check_path
+ from mlp.utils import check_path
print "## Begin validation of the final motion (collision and joint-limits)"
validator = check_path.PathChecker(v,cp.fullBody,cs_com,cfg.nq,True)
motion_valid,t_invalid = validator.check_motion(res.q_t)
@@ -86,14 +86,14 @@ else :
motion_valid = True
if cfg.PLOT:
- from hpp_wholebody_motion.utils import plot
+ from mlp.utils import plot
plot.plotALLFromWB(cs_com,res)
if cfg.EXPORT_OPENHRP and motion_valid:
- from hpp_wholebody_motion.export import openHRP
+ from mlp.export import openHRP
openHRP.export(cs_com,res)
if cfg.EXPORT_GAZEBO and motion_valid:
- from hpp_wholebody_motion.export import gazebo
+ from mlp.export import gazebo
gazebo.export(res.q_t)
--
GitLab