diff --git a/scripts/hpp_wholebody_motion/export/openHRP.py b/scripts/hpp_wholebody_motion/export/openHRP.py
index 5e98c7b3a8d912261a531e739cd3cd259a5024fe..716cd961ba9219e6fcbff1a76086147324f5aa25 100644
--- a/scripts/hpp_wholebody_motion/export/openHRP.py
+++ b/scripts/hpp_wholebody_motion/export/openHRP.py
@@ -10,10 +10,13 @@ import numpy as np
from rospkg import RosPack
#import tsid # required for robot wrapper ! FIXME
from pinocchio.robot_wrapper import RobotWrapper
+from hpp_wholebody_motion.utils.computation_tools import *
class Struct():
None
+DT = 0.005
+
# hardcoded ID for HRP-2 :
ff = range(7)
chest = range(7,9)
@@ -24,13 +27,29 @@ l_leg = range(25,31)
r_leg = range(31,37)
-def computeResultTrajectory(robot, t_t, q_t, v_t, a_t):
-
+def computeResultTrajectory(robot,cs, q_t_list, v_t_list, a_t_list):
+ # start by adapting the time step between the IK and the the one of open hrp (0.005)
+ assert DT>= cfg.IK_dt and "The dt used when computing q(t) must be superior or equal to 0.005 for this export."
+ assert (DT % cfg.IK_dt)==0 and "The dt used when computing q(t) must be a multiple of 0.005."
+ n = int(DT/cfg.IK_dt)
+ N_ik = len(q_t_list)
+ N = N_ik/n
+ #print "Export openHRP, ratio between dt : ",n
+ #print "Num point for IK : ",N_ik
+ #print "Num point for openHRP : ",N
+ q_t = np.matrix(np.zeros([q_t_list[0].shape[0],N]))
+ v_t = np.matrix(np.zeros([v_t_list[0].shape[0],N]))
+ a_t = np.matrix(np.zeros([a_t_list[0].shape[0],N]))
+ for i in range(N):
+ q_t[:,i] = q_t_list[i*n]
+ v_t[:,i] = v_t_list[i*n]
+ a_t[:,i] = a_t_list[i*n]
+ # build timeline vector :
+ t_t = [i*DT for i in range(q_t.shape[1])]
+ #print "timeline size : ",len(t_t)
model = robot.model
data = robot.data
- N = q_t.shape[1]
-
ZMP_t = np.matrix(np.zeros([3,N]))
waist_t = np.matrix(np.zeros([3,N]))
pcom_t = np.matrix(np.empty([3,N]))
@@ -78,14 +97,7 @@ def computeResultTrajectory(robot, t_t, q_t, v_t, a_t):
#phi0 = oXi_s * (M[:6,:] * a + b[:6])
tau_t[:,k] = data.tau
phi0 = data.oMi[1].act(se3.Force(data.tau[:6]))
-
- wrench_t[:,k] = phi0.vector
-
- forces = wrench_t[:3,k]
- torques = wrench_t[3:,k]
-
- ZMP_t[0,k] = -torques[1]/forces[2]
- ZMP_t[1,k] = torques[0]/forces[2]
+ ZMP_t[:,k] = shiftZMPtoFloorAltitude(cs,t_t[k],phi0)
waist_t[:,k] = robot.data.oMi[1].translation
waist_orientation_t[:,k] = matrixToRpy(robot.data.oMi[1].rotation)
@@ -164,11 +176,12 @@ def generateOpenHRPMotion(robot, data, path, project_name):
line += delim + str(vector[k,0])
return line
-
- timeline = data.t_t - data.t_t[0]
+
+ timeline = data.t_t
N = len(timeline)
- dt = 0.005
- timeline = np.linspace(0., (N-1)*dt, N)
+ print "in generateOpenHRPMotion, N= ",N
+ dt = DT
+
delim = " "
eol = "\n"
@@ -260,10 +273,10 @@ def writeKinematicsData(robot, data, path, project_name):
return line
- timeline = data.t_t - data.t_t[0]
+ timeline = data.t_t
N = len(timeline)
- dt = 0.005
- timeline = np.linspace(0., (N-1)*dt, N)
+ print "in write kinematic, number of points : ",N
+ dt = DT
delim = " "
eol = "\n"
@@ -309,10 +322,8 @@ def writeKinematicsData(robot, data, path, project_name):
print "write file : ",filename_acc
-def export(q_t,v_t,a_t,t_t = None):
- if not t_t :
- t_t = [i*cfg.IK_dt for i in range(len(q_t))]
- assert len(q_t) == len(v_t) and len(q_t) == len(a_t) and len(q_t) == len(t_t) and "all states vector must have the same size"
+def export(cs,q_t,v_t,a_t):
+ assert len(q_t) == len(v_t) and len(q_t) == len(a_t) and "all states vector must have the same size"
rp = RosPack()
urdf = rp.get_path(cfg.Robot.packageName)+'/urdf/'+cfg.Robot.urdfName+cfg.Robot.urdfSuffix+'.urdf'
if cfg.WB_VERBOSE:
@@ -321,35 +332,9 @@ def export(q_t,v_t,a_t,t_t = None):
robot = RobotWrapper(urdf, se3.StdVec_StdString(), se3.JointModelFreeFlyer(), False)
if cfg.WB_VERBOSE:
print "robot loaded in export OpenHRP"
- # convert list of states to matrices (line : value, col : id)
- N = len(q_t)
- q_t_mat = np.matrix(np.zeros([q_t[0].shape[0],N]))
- v_t_mat = np.matrix(np.zeros([v_t[0].shape[0],N]))
- a_t_mat = np.matrix(np.zeros([a_t[0].shape[0],N]))
- t_t_mat = np.matrix(t_t)
- for i in range(N):
- q_t_mat[:,i] = q_t[i]
- v_t_mat[:,i] = v_t[i]
- a_t_mat[:,i] = a_t[i]
-
-
- results_sot = computeResultTrajectory(robot, t_t_mat, q_t_mat, v_t_mat, a_t_mat)
- """
- ZMP_t = results_sot.ZMP_t.copy()
- pcom_t = results_sot.pcom_t
- # Set zmp trajectory to be the reference one
- Wrench_ref_trajectory = DifferentiableEuclidianTrajectory()
- Wrench_ref_trajectory.computeFromPoints(trajs.time_t,F0_t,0*F0_t)
-
- Wrench_trajectory = DifferentiableEuclidianTrajectory()
- Wrench_trajectory.computeFromPoints(np.asmatrix(results_sot.t_t),results_sot.wrench_t,0*results_sot.wrench_t)
-
- ZMP_ref_t = computeRefZMP(cs,results_sot.t_t,Wrench_ref_trajectory)
- ZMP_t = computeRefZMP(cs,results_sot.t_t,Wrench_trajectory)
-
- results_sot.ZMP_t[:,:] = ZMP_t
- """
+
+ results = computeResultTrajectory(robot,cs, q_t, v_t, a_t)
path = cfg.EXPORT_PATH+"/openHRP"
- q_openhrp_l = generateOpenHRPMotion(robot, results_sot, path, cfg.DEMO_NAME)
- writeKinematicsData(robot, results_sot, path, cfg.DEMO_NAME)
+ q_openhrp_l = generateOpenHRPMotion(robot, results, path, cfg.DEMO_NAME)
+ writeKinematicsData(robot, results, path, cfg.DEMO_NAME)
\ No newline at end of file
diff --git a/scripts/hpp_wholebody_motion/utils/computation_tools.py b/scripts/hpp_wholebody_motion/utils/computation_tools.py
new file mode 100644
index 0000000000000000000000000000000000000000..5d869b1bbe7251f2430951c37cbc5cfc11521544
--- /dev/null
+++ b/scripts/hpp_wholebody_motion/utils/computation_tools.py
@@ -0,0 +1,159 @@
+
+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
+### tools to compute zmp trajectory : ###
+
+
+def pacthSameAltitude(M1,M2,eps=1e-3):
+ return abs(M1.translation[2] == M2.translation[2]) <= eps
+
+# compute an approximation of the floor altitude (used for the zmp reference)
+#Only consider the feet :
+# - if only one feet in contact, return it's z coordinate
+# - if both feet in contact, make a linear interp between both feet altitude wrt to which feet will move next
+def computeFloorAltitude(cs,t):
+ id_phase = findPhase(cs,t)
+ phase = cs.contact_phases[id_phase]
+ RF_patch = phase.RF_patch
+ LF_patch = phase.LF_patch
+ Mrf = RF_patch.placement
+ Mlf = LF_patch.placement
+
+ if RF_patch.active and LF_patch.active:
+ if pacthSameAltitude(Mrf,Mlf):
+ floor_altitude = 0.5*(Mrf.translation+Mlf.translation)[2]
+ else:
+ # look for wich feet just created contact :
+ LF_moved = False
+ if id_phase > 0: #look for the inactive contact in the previous phase
+ pprev = cs.contact_phases[id_phase-1]
+ if not pprev.RF_patch.active:
+ LF_moved = False
+ elif not pprev.LF_patch.active:
+ LF_moved = True
+ else:
+ assert "Must never happened"
+ else: #look for the inactive contact in the next phase and assume cyclic gait for the last couple of phases
+ pnext = cs.contact_phases[id_phase+1]
+ if not pnext.RF_patch.active:
+ LF_moved = True
+ elif not pnext.LF_patch.active :
+ LF_moved = False
+ else:
+ assert "Must never happened"
+ # linear interp of the altitude (from the feet which are going to move to the one that stay)
+ if LF_moved:
+ p0 = Mrf.translation[2]
+ p1 = Mlf.translation[2]
+ else:
+ p0 = Mlf.translation[2]
+ p1 = Mrf.translation[2]
+
+ t0 = phase.time_trajectory[0]
+ t1 = phase.time_trajectory[-1]
+ assert t0 < t1
+ s = (t - t0) / (t1-t0)
+ floor_altitude = p0 + s * (p1-p0)
+ pass
+ elif RF_patch.active:
+ floor_altitude = Mrf.translation[2]
+ elif LF_patch.active:
+ floor_altitude = Mlf.translation[2]
+ else:
+ assert "Must never happened"
+ return floor_altitude
+
+def shiftZMPtoFloorAltitude(cs,t,phi0):
+ Mshift = SE3.Identity()
+ shift = Mshift.translation
+ floor_altitude = computeFloorAltitude(cs,t)
+ floor_altitude -= cfg.Robot.dict_offset[cfg.Robot.rfoot].translation[2][0] # shift to join level, assume both feet have the same transform
+ shift[2] = floor_altitude
+ Mshift.translation = shift
+
+ # apply transform to wrench :
+ phi_floor = Mshift.actInv(phi0)
+ #compute zmp with transformed phi :
+ w = phi_floor.angular
+ f = phi_floor.linear
+ #print "Zx",-w[1]/f[2]
+ #print "Zy",w[0]/f[2]
+ #print floor_altitude
+ ZMP = np.matrix([float(-w[1]/f[2]),float(w[0]/f[2]),float(floor_altitude)]).T
+ return ZMP
+
+
+# not generic ! only consider feet
+# (not a problem as we only call it for openHRP)
+def computeRefZMP(cs,time_t,Wrench_traj):
+
+ N = len(time_t)
+ ZMP_t = np.matrix(np.empty((3,N)))
+ Mshift = SE3.Identity()
+ shift = Mshift.translation
+
+ phase0 = cs.contact_phases[0]
+ phase1 = cs.contact_phases[1]
+
+ if phase1.RF_patch.active:
+ RF_SS_first = True
+ else:
+ RF_SS_first = False
+
+ for k,t in enumerate(time_t):
+
+ id_phase = findPhase(cs,t)
+ phase = cs.contact_phases[id_phase]
+ wrench = Wrench_traj(t)[0]
+ phi0 = Force(wrench)
+
+ RF_patch = phase.RF_patch
+ LF_patch = phase.LF_patch
+
+ Mrf = RF_patch.placement
+
+ Mlf = LF_patch.placement
+
+ if RF_patch.active and LF_patch.active:
+ if pacthSameAltitude(Mrf,Mlf):
+ floor_altitude = 0.5*(Mrf.translation+Mlf.translation)[2]
+ else:
+
+ if RF_SS_first:
+ p0 = Mrf.translation[2]
+ p1 = Mlf.translation[2]
+ else:
+ p0 = Mlf.translation[2]
+ p1 = Mrf.translation[2]
+
+ t0 = phase.time_trajectory[0]
+ t1 = phase.time_trajectory[-1]
+ assert t0 < t1
+ s = (t - t0) / (t1-t0)
+ floor_altitude = p0 + s * (p1-p0)
+ pass
+ elif RF_patch.active:
+ floor_altitude = Mrf.translation[2]
+ elif LF_patch.active:
+ floor_altitude = Mlf.translation[2]
+ else:
+ assert "Must never happened"
+
+
+ shift[2] = floor_altitude
+ Mshift.translation = shift
+
+ phi_floor = Mshift.actInv(phi0)
+ w = phi_floor.angular
+ f = phi_floor.linear
+ #print "Zx",-w[1]/f[2]
+ #print "Zy",w[0]/f[2]
+ #print floor_altitude
+ ZMP = np.matrix([float(-w[1]/f[2]),float(w[0]/f[2]),float(floor_altitude)]).T
+ ZMP_t[:,k] = ZMP
+
+ return ZMP_t
+
diff --git a/scripts/hpp_wholebody_motion/utils/util.py b/scripts/hpp_wholebody_motion/utils/util.py
index 95bf271efc80de2a6631810998bf6e83e614189e..82b898efeb11685014513318dff4174a60b5438a 100644
--- a/scripts/hpp_wholebody_motion/utils/util.py
+++ b/scripts/hpp_wholebody_motion/utils/util.py
@@ -21,4 +21,19 @@ def distPointLine(p_l,x1_l,x2_l):
return norm(cross(p-x1,p-x2)) / norm(x2-x1)
-
\ No newline at end of file
+
+def findPhase(cs,t):
+ phase0 = cs.contact_phases[0]
+ phasel = cs.contact_phases[-1]
+ if t <= phase0.time_trajectory[0]:
+ return 0
+ elif t >= phasel.time_trajectory[-1]:
+ return len(cs.contact_phases)-1
+
+
+ id = [k for k, phase in enumerate(cs.contact_phases) if t >= phase.time_trajectory[0] and t <= phase.time_trajectory[-1]]
+ assert len(id)>=1 or len(id) <= 2
+ if len(id) == 2:
+ return id[1]
+ else:
+ return id[0]
\ No newline at end of file