diff --git a/scripts/hpp_wholebody_motion/config.py b/scripts/hpp_wholebody_motion/config.py
index faab96fb2ef508e88daa1f073a0d2c618867c3d2..cac693cbef1502ebda2f2a16b9a2fdc798816e90 100644
--- a/scripts/hpp_wholebody_motion/config.py
+++ b/scripts/hpp_wholebody_motion/config.py
@@ -9,7 +9,8 @@ LOAD_CS = False
LOAD_CS_COM = False
SAVE_CS = not LOAD_CS and True
SAVE_CS_COM = not LOAD_CS_COM and True
-EXPORT_GAZEBO = True
+EXPORT_GAZEBO = False
+EXPORT_OPENHRP = True
EXPORT_PATH = OUTPUT_DIR+"/export"
##DISPLAY settings :
@@ -19,13 +20,13 @@ DISPLAY_INIT_GUESS_TRAJ = False
DISPLAY_WP_COST=True
DISPLAY_COM_TRAJ = True
DISPLAY_FEET_TRAJ = True
-DISPLAY_WB_MOTION = True
+DISPLAY_WB_MOTION = False
DT_DISPLAY = 0.05 # dt used to display the wb motion
### Settings for generate_contact_sequence
FORCE_STRAIGHT_LINE = False # DEBUG ONLY should be false
-### Settings for locomote script :
+### Settings for centroidal script :
MU=0.5
USE_GEOM_INIT_GUESS = True
USE_CROC_INIT_GUESS = False
@@ -37,19 +38,22 @@ COM_SHIFT_Z = 0.0
TIME_SHIFT_COM = 0.0
USE_WP_COST = False # use wp from the contact sequence in the cost function of timeopt
-#end effector :
+
+## Settings for end effector :
USE_LIMB_RRT = False
USE_CONSTRAINED_BEZIER = True
USE_BEZIER_EE = True
EFF_CHECK_COLLISION = True
+WB_ABORT_WHEN_INVALID = True
+
## Settings for whole body :
YAW_ROT_GAIN = 1.
USE_CROC_COM = False
-WB_VERBOSE = True
+WB_VERBOSE = False
WB_STOP_AT_EACH_PHASE = False
IK_dt = 0.001 # controler time step
IK_PRINT_N = 500 # print state of the problem every IK_PRINT_N time steps (if verbose = True)
-
+CHECK_FINAL_MOTION = True
# import specific settings for the selected demo. This settings may override default ones.
import importlib
diff --git a/scripts/hpp_wholebody_motion/utils/check_path.py b/scripts/hpp_wholebody_motion/utils/check_path.py
index 6c66fc99f1ad162dd3b6d6abd0f70baaf8be60cd..57b961e72dba2aeaad634153371e330660c0ff66 100644
--- a/scripts/hpp_wholebody_motion/utils/check_path.py
+++ b/scripts/hpp_wholebody_motion/utils/check_path.py
@@ -1,36 +1,72 @@
# Copyright 2018, LAAS-CNRS
# Author: Pierre Fernbach
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
-from config import *
-
+import hpp_wholebody_motion.config as cfg
import numpy as np
-def pinnochioQuaternion(q):
- assert len(q)>6, "config vector size must be superior to 7"
- w = q[3]
- q[3:6] = q[4:7]
- q[6] = w
- return q
-
-def HPPQuaternion(q):
- assert len(q)>6, "config vector size must be superior to 7"
- w = q[6]
- q[4:7] = q[3:6]
- q[3] = w
- return q
-
class PathChecker():
- def __init__(self,r,fullBody,cs,q_t,t_t):
+ def __init__(self,r,fullBody,cs,sizeQ,verbose = False):
self.r = r
self.cs = cs
- self.q_t = q_t
- self.t_t = t_t
- self.size_conf = q_t.shape[0]
self.fullBody = fullBody # with effector collision disabled
+ self.sizeQ = sizeQ
+ self.configSize = fullBody.getConfigSize()
+ self.dt = cfg.IK_dt
+ self.verbose = verbose
+ self.extraDof = int(fullBody.client.robot.getDimensionExtraConfigSpace())
+
+ # convert to correct format for hpp, add extra dof if necessary
+ # return valid,message : valid = bool, message = string
+ def checkConfig(self,q_m):
+ q = [0]*self.configSize
+ q[:self.sizeQ] = q_m.T.tolist()[0]
+ res = self.fullBody.isConfigValid(q)
+ return res[0],res[1]
+
+
+
+ def qAtT(self,t,q_t):
+ for it in range(1,len(q_t)-1):
+ if t>(self.dt*(it-0.5)) and t<=(self.dt*(it+0.5)):
+ return q_t[it]
+
+
+
+ def phaseOfT(self,t_switch):
+ for i in range(self.cs.size()):
+ p = self.cs.contact_phases[i]
+ if t_switch>=p.time_trajectory[0] and t_switch<=p.time_trajectory[-1]:
+ return i
+
+
+ def phaseOfId(self,id):
+ t_switch = self.dt*float(id)
+ return self.phaseOfT(t_switch)
+
+ # return a bool (true = valid) and the time of the first invalid q (None if always valid):
+ # if Verbose = True : check the complete motion before returning,
+ # if False stop at the first invalid
+ def check_motion(self,q_t):
+ always_valid = True
+ first_invalid = None
+ for i in range(len(q_t)):
+ valid,mess = self.checkConfig(q_t[i])
+ if not valid :
+ if always_valid : # first invalid config
+ always_valid = False
+ first_invalid = self.dt*(float(i))
+ if self.verbose:
+ print "Invalid config at t= ",self.dt*(float(i))
+ print mess
+ else:
+ return always_valid,first_invalid
+ return always_valid,first_invalid
+
+ ############# old stuffs (need to be updated if necessary : #############
+ """
def check_postures(self,verbose = True):
bad_phases = []
success_global=True
@@ -149,34 +185,12 @@ class PathChecker():
q[:self.size_conf] = self.q_t[:,id].transpose().tolist()[0]
q = HPPQuaternion(q)
self.r(q)
-
- def qAtT(self,t):
- for it in range(1,len(self.t_t)-1):
- if t>((self.t_t[it]+self.t_t[it-1])/2.) and t<=((self.t_t[it]+self.t_t[it+1])/2.):
- q = [0]*self.fullBody.getConfigSize()
- q[:self.size_conf] = self.q_t[:,it].transpose().tolist()[0]
- q = HPPQuaternion(q)
- #print "closer t : ",self.t_t[it]
- #print "at id : ",it
- return q
+
def showConfigAtTime(self,t):
q = self.qAtT(t)
self.r(q)
-
- def phaseOfT(self,t_switch):
- for i in range(self.cs.size()):
- p = self.cs.contact_phases[i]
- if t_switch>=p.time_trajectory[0] and t_switch<=p.time_trajectory[-1]:
- return i
-
-
- def phaseOfId(self,id):
- t_switch = self.t_t[id]
- return self.phaseOfT(t_switch)
-
-
-
+
def checkAdditionalJointsConstraints(self,list_of_id,mins,maxs):
valid = True
@@ -191,4 +205,4 @@ class PathChecker():
return valid
-
\ No newline at end of file
+ """
\ No newline at end of file
diff --git a/scripts/run_locomote.py b/scripts/run_locomote.py
index 8b1616338ab7a7de1a955c25923f032af4d2a78e..9f564ce6437b21fd2e920038ca6b0c8ccfeefeee 100644
--- a/scripts/run_locomote.py
+++ b/scripts/run_locomote.py
@@ -73,6 +73,25 @@ if cfg.DISPLAY_WB_MOTION:
raw_input("Press Enter to display the whole body motion ...")
display_tools.displayWBmotion(v,q_t,cfg.IK_dt,cfg.DT_DISPLAY)
+if cfg.CHECK_FINAL_MOTION :
+ from hpp_wholebody_motion.utils import check_path
+ print "## Begin validation of the final motion (collision and joint-limits)"
+ validator = check_path.PathChecker(v,cp.fullBody,cs_com,len(q_t[0]),True)
+ motion_valid,t_invalid = validator.check_motion(q_t)
+ print "## Check final motion, valid = ",motion_valid
+ if not motion_valid:
+ print "## First invalid time : ",t_invalid
+else :
+ motion_valid = True
+
+
+if cfg.EXPORT_OPENHRP and motion_valid:
+ from hpp_wholebody_motion.export import openHRP
+ openHRP.export(q_t,v_t,a_t)
+if cfg.EXPORT_GAZEBO and motion_valid:
+ from hpp_wholebody_motion.export import gazebo
+ gazebo.export(q_t)
+