From c481cf10b22aeeaf7fce7dde510b20ecfb93565d Mon Sep 17 00:00:00 2001
From: stevet <stevetonneau@gotmail.fr>
Date: Wed, 29 May 2019 16:20:58 +0200
Subject: [PATCH] added specific start end state setters with normals, with
test on hyq3d
---
script/scenarios/demos/darpa_hyq3d.py | 122 +++++++++++++++++++++
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 43 ++++++--
src/rbprmbuilder.impl.cc | 5 +
3 files changed, 158 insertions(+), 12 deletions(-)
create mode 100644 script/scenarios/demos/darpa_hyq3d.py
diff --git a/script/scenarios/demos/darpa_hyq3d.py b/script/scenarios/demos/darpa_hyq3d.py
new file mode 100644
index 0000000..abbc8db
--- /dev/null
+++ b/script/scenarios/demos/darpa_hyq3d.py
@@ -0,0 +1,122 @@
+#Importing helper class for RBPRM
+from hpp.corbaserver.rbprm.hyq import Robot
+from hpp.corbaserver.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+#calling script darpa_hyq_path to compute root path
+import darpa_hyq_path as tp
+
+from os import environ
+ins_dir = environ['DEVEL_HPP_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+
+from hpp.corbaserver import Client
+
+
+
+# This time we load the full body model of HyQ
+fullBody = Robot ()
+fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
+fullBody.client.robot.setDimensionExtraConfigSpace(6)
+fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
+
+# Setting a number of sample configurations used
+nbSamples = 10000
+
+ps = tp.ProblemSolver(fullBody)
+v = tp.Viewer (ps, viewerClient=tp.v.client)
+
+rootName = 'base_joint_xyz'
+cType = "_3_DOF"
+
+def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
+ fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
+
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.1, cType)
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
+fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
+fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
+
+#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
+#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
+
+q_init=fullBody.referenceConfig[::] + [0]*6; q_init[0:7] = tp.q_init[0:7];
+#q_init[2]=fullBody.referenceConfig[2]
+q_goal = fullBody.referenceConfig[::]+ [0]*6; q_goal[0:7] = tp.q_goal[0:7];
+#q_goal[2]=fullBody.referenceConfig[2]
+
+
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+# specifying the full body configurations as start and goal state of the problem
+fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId], [[0.,0.,1.] for _ in range(4)])
+fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId], [[0.,0.,1.] for _ in range(4)])
+
+
+
+v(q_init)
+configs = []
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client, v)
+
+
+import time
+
+#DEMO METHODS
+
+def initConfig():
+ v.client.gui.setVisibility("hyq", "ON")
+ tp.cl.problem.selectProblem("default")
+ tp.v.client.gui.setVisibility("toto", "OFF")
+ tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
+ v(q_init)
+
+def endConfig():
+ v.client.gui.setVisibility("hyq", "ON")
+ tp.cl.problem.selectProblem("default")
+ tp.v.client.gui.setVisibility("toto", "OFF")
+ tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
+ v(q_goal)
+
+
+def rootPath():
+ v.client.gui.setVisibility("hyq", "OFF")
+ tp.cl.problem.selectProblem("rbprm_path")
+ tp.v.client.gui.setVisibility("toto", "OFF")
+ v.client.gui.setVisibility("hyq", "OFF")
+ tp.v.client.gui.setVisibility("hyq_trunk_large", "ON")
+ tp.pp(0)
+ tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
+ v.client.gui.setVisibility("hyq", "ON")
+ tp.cl.problem.selectProblem("default")
+
+def genPlan(stepsize=0.06):
+ tp.cl.problem.selectProblem("default")
+ v.client.gui.setVisibility("hyq", "ON")
+ tp.v.client.gui.setVisibility("toto", "OFF")
+ tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
+ global configs
+ start = time.time()
+ configs = fullBody.interpolate(stepsize, 5, 0., filterStates = True,testReachability = False)
+ end = time.time()
+ print "Contact plan generated in " + str(end-start) + "seconds"
+
+def contactPlan(step = 0.5):
+ v.client.gui.setVisibility("hyq", "ON")
+ tp.cl.problem.selectProblem("default")
+ tp.v.client.gui.setVisibility("toto", "OFF")
+ tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
+ for i in range(0,len(configs)):
+ v(configs[i]);
+ time.sleep(step)
+
+
+print "Root path generated in " + str(tp.t) + " ms."
+
+genPlan(0.01);contactPlan(0.01)
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 10b07a8..9af41f9 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -262,14 +262,41 @@ class FullBody (Robot):
# \param sampleId id of the considered sample
def getSampleValue(self, limbName, valueName, sampleId):
return self.clientRbprm.rbprm.getSampleValue(limbName, valueName, sampleId)
-
+
## Initialize the first configuration of the path interpolation
# with a balanced configuration for the interpolation problem;
#
# \param configuration the desired start configuration
# \param contacts the array of limbs in contact
- def setStartState(self, configuration, contacts):
- return self.clientRbprm.rbprm.setStartState(configuration, contacts)
+ # \param normals the array describing one normal direction for each limb in contact
+ def setStartState(self, configuration, contacts, normals = None):
+ if normals is None:
+ return self.clientRbprm.rbprm.setStartState(configuration, contacts)
+ cl = self.clientRbprm.rbprm
+ sId = cl.createState(configuration, contacts)
+ num_max_sample = 1
+ for (limbName, normal) in zip(contacts, normals):
+ p = cl.getEffectorPosition(limbName,configuration)[0]
+ cl.addNewContact(sId, limbName, p, normal, num_max_sample)
+ return cl.setStartStateId(sId)
+
+
+ ## Initialize the goal configuration of the path interpolation
+ # with a balanced configuration for the interpolation problem;
+ #
+ # \param configuration the desired goal configuration
+ # \param contacts the array of limbs in contact
+ # \param normals the array describing one normal direction for each limb in contact
+ def setEndState(self, configuration, contacts, normals = None):
+ if normals is None:
+ return self.clientRbprm.rbprm.setEndState(configuration, contacts)
+ cl = self.clientRbprm.rbprm
+ sId = cl.createState(configuration, contacts)
+ num_max_sample = 1
+ for (limbName, normal) in zip(contacts, normals):
+ p = cl.getEffectorPosition(limbName,configuration)[0]
+ cl.addNewContact(sId, limbName, p, normal, num_max_sample)
+ return cl.setEndStateId(sId)
## Initialize the first state of the path interpolation
# \param stateId the Id of the desired start state in fullBody
@@ -289,15 +316,7 @@ class FullBody (Robot):
# \return id of created state
def createState(self, configuration, contacts):
return self.clientRbprm.rbprm.createState(configuration, contacts)
-
- ## Initialize the last configuration of the path discretization
- # with a balanced configuration for the interpolation problem;
- #
- # \param configuration the desired end configuration
- # \param contacts the array of limbs in contact
- def setEndState(self, configuration, contacts):
- return self.clientRbprm.rbprm.setEndState(configuration, contacts)
-
+
## Saves a computed contact sequence in a given filename
#
# \param The file where the configuration must be saved
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 3fe976b..4528ca4 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -1254,6 +1254,11 @@ namespace hpp {
throw std::runtime_error ("Impossible to find limb for joint "
+ (*cit) + " to robot; limb not defined");
}
+ if(lit->second->contactType_ == _3_DOF)
+ {
+ throw std::runtime_error ("Can't set contact normal to a limb with 3D contact: "
+ + (*cit) + ". The normal must be provided using method setStartState / setEndState (self, configuration, contacts, normals)");
+ }
const pinocchio::Frame frame = fullBody->device_->getFrameByName(lit->second->effector_.name());
const pinocchio::Transform3f& transform = frame.currentTransformation ();
const fcl::Matrix3f& rot = transform.rotation();
--
GitLab