From 1a32f5df1bb30e5b91a41ed5b3c11ee48bfb6d24 Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Mon, 25 Mar 2019 14:01:31 +0100
Subject: [PATCH] [wholebody] correctly check the motion for collision and
joint limits at each phase
---
.../wholebody/tsid_invdyn.py | 25 ++++++++++++-------
1 file changed, 16 insertions(+), 9 deletions(-)
diff --git a/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py b/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py
index 39e745b..ccf69d4 100644
--- a/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py
+++ b/scripts/hpp_wholebody_motion/wholebody/tsid_invdyn.py
@@ -243,6 +243,10 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
+ if cfg.EFF_CHECK_COLLISION : # initialise object needed to check the motion
+ from hpp_wholebody_motion.utils import check_path
+ validator = check_path.PathChecker(viewer,fullBody,cs,len(q),False)
+
if cfg.WB_VERBOSE:
print "initialize tasks : "
comTask = tsid.TaskComEquality("task-com", robot)
@@ -408,7 +412,7 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
# end effector (if they exists)
for eeName,traj in dic_effectors_trajs.iteritems():
if traj:
- swingPhase = True
+ swingPhase = True # there is an effector motion in this phase
sampleEff = effectorTraj.computeNext()
sampleEff.pos(SE3toVec(traj(t)[0]))
sampleEff.vel(MotiontoVec(traj(t)[1]))
@@ -456,16 +460,19 @@ def generateWholeBodyMotion(cs,viewer=None,fullBody=None):
return q_t+q_t_phase, v_t+v_t_phase, a_t+a_t_phase
# end while t \in phase_t (loop for the current contact phase)
if swingPhase and cfg.EFF_CHECK_COLLISION :
- phaseValid = iter_for_phase >= 1 #TODO : check collision here instead for this debug code
- phaseValid = True # DEBUG FIXME
+ phaseValid,t_invalid = validator.check_motion(q_t_phase)
if not phaseValid :
if cfg.WB_VERBOSE :
- print "Phase "+str(pid)+" not valid, try new end effector trajectory."
- for eeName,oldTraj in dic_effectors_trajs.iteritems():
- if oldTraj: # update the traj in the map
- ref_traj = generateEEReferenceTrajCollisionFree(fullBody,robot,invdyn.data(),t_begin,phase_prev,phase,phase_next,q_t_phase,oldTraj,eeName,pid,viewer)
- dic_effectors_trajs.update({eeName:ref_traj})
- else : # no effector motions, phase always valid
+ print "Phase "+str(pid)+" not valid at t = "+ t_invalid
+ if cfg.WB_ABORT_WHEN_INVALID :
+ return q_t,v_t,a_t
+ else :
+ print "Try new end effector trajectory."
+ for eeName,oldTraj in dic_effectors_trajs.iteritems():
+ if oldTraj: # update the traj in the map
+ ref_traj = generateEEReferenceTrajCollisionFree(fullBody,robot,invdyn.data(),t_begin,phase_prev,phase,phase_next,q_t_phase,oldTraj,eeName,pid,viewer)
+ dic_effectors_trajs.update({eeName:ref_traj})
+ else : # no effector motions, phase always valid (or bypass the check)
phaseValid = True
if cfg.WB_VERBOSE :
print "Phase "+str(pid)+" valid."
--
GitLab