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 de104aa67b34a89cd80ac15b1790d0d53a420d94..d3a28241815f33f9550b5383b9ea5c5cb90fec1a 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 1702d9578d033d3cc024835a11b0ab07ec60175c..041dedf34334fb5ff51317d30f8ccb313e6f9a1e 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 e6b1de5414df68a6de888615f09a3425f4df6abd..c9cd64180b82465ab6d624cc5c7be0390ab17c68 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 d5b0644a187cb61b1e807bcde57a542c962b1b41..293735978bb5bb344733c7c9ffa9a880af176dff 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 9e6beec218730370f9e63a279aad0293df511b42..d2b5562dc70649ca6b8774588ae7e1d93182a7b7 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 0000000000000000000000000000000000000000..d96ae6d3063357960c51fb7902942334369135b2
--- /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 0000000000000000000000000000000000000000..a7b5682538542c2e72442bd078c52be2e38320af
--- /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 7af032de5d0d4effd7218608e05594f29511647d..46c4e758e81b4b7f22259a5722165a98ad49ee3a 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 0cd817e14f213f5123740452b0176efba54bd64e..129c3924f47955aa801b898214c7d546dbcd3e9e 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 c0f2b376f573d6d3cd81dff2a8a0e30aa6ba25ea..46d2b4a565dd91ebe52d675b35d31af9a6d414ee 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 ce0d9d1a5cebdd6a05555a1afd2b669dd333cc57..0d11462bb388b0e7da68f0fc4f0d1068adf6d120 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 416a6033fd17ee66a249e6bb5fd61dc06362dee3..13f196952f329161479f79b81ec8d7490e5c2e6f 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 c3620434e9c7febe57f355acd10ca8b3666331ad..9e349513af951e76b671e6305f4c81492bf04dad 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 96add52f11b4ae7daa379efe1144fa52b3a6f54c..ad87d582322ceb8b272b1847165eb3ec9982afe2 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 f75e1bd620b103aa973caccdc0148aaf9641378a..82a5c9adc08de2e74f70b9f735364707ef06c8b5 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 cf5a77ee88d25c4a388f48beb917b4bf942eff6f..1402c8ff6c977f46d93063f20bdbc98c73f16604 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 fb8b1444af40956cdd5f11a57305564722fb100a..bd861a6397e4cb893a0ec2bf1aba4e31fe8d642f 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 f25e2e5a4357fcfba09da209e880ad350237121b..88c04e10566c2dcae39b6cda21e87c6420bc6fe7 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 37bdbee50ab843fa9b11f96c233c1d72854f14a1..14b1836f4602482626bc40c6a8626ed097a3f83f 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 f892d47fee79e9c81faef54fc12a879b80b6917c..631b6aff1918980ebe7944caf374ad59cae1a118 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 16eaae0243f6a55b8021cf3b3fc3acc28931957f..b90ba4e1af8b0e4c418cb4fee18bde279fbaffd6 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 ccadb75e88156ef56cdae5d7f7d7e13457ae0db7..ed0a7c7252d77bb47c043ea6a06a224422314456 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)