From c9d2b1107fd9616c3505a1b6e4ef33ffbfed1fb8 Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Tue, 12 Mar 2019 13:55:39 +0100
Subject: [PATCH] [wb] fix index of states list
---
.../hpp_wholebody_motion/wholebody/tsid_invdyn.py | 13 +++++++------
1 file changed, 7 insertions(+), 6 deletions(-)
diff --git a/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py b/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py
index 6604297..39e745b 100644
--- a/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py
+++ b/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py
@@ -235,9 +235,10 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
q = cs.contact_phases[0].reference_configurations[0].copy()
v = np.matrix(np.zeros(robot.nv)).transpose()
t = 0.0 # time
- q_t = []
- v_t = []
- a_t = []
+ # init states list with initial state (assume joint velocity is null for t=0)
+ q_t = [q.copy()]
+ v_t = [v.copy()]
+ a_t = [] # fill 'a' one index before v and q : a[i] is the acc needed to go from v[i] to v[i+1]
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
@@ -385,7 +386,7 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
print "t begin = ",t_begin
print "current t = ",t
# loop to generate states (q,v,a) for the current contact phase :
- while t < phase.time_trajectory[-1] - dt :
+ while (t < phase.time_trajectory[-1] - dt) or (t <= phase.time_trajectory[-1] and pid == cs.size()-1) :
# set traj reference for current time :
# com
sampleCom = trajCom.computeNext()
@@ -441,7 +442,7 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
t += dt
# store current state
q_t_phase += [q]
- v_t_phase += [v]
+ v_t_phase += [v.copy()]
a_t_phase += [dv]
if norm(dv) > 1e6 or norm(v) > 1e6 :
print "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
@@ -486,7 +487,7 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
if cfg.WB_VERBOSE:
print "\nFinal COM Position ", robot.com(invdyn.data()).T
print "Desired COM Position", cs.contact_phases[-1].final_state.T
-
+ a_t += [np.matrix(np.zeros(robot.nv)).transpose()] # add last value of 'a' (to have the same size as v and q)
return q_t,v_t,a_t
--
GitLab