Commit c481cf10 authored by stevet's avatar stevet
Browse files

added specific start end state setters with normals, with test on hyq3d

parent 462419ee
#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)
......@@ -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
......
......@@ -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();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment