def __init__(self, robot, start, end):
Client.__init__(self)
self.repeat = 0
# print 'creating an agent of type ', robotType
self.robot = robot
self.start_config = start
self.end_config = end
self.current_config = self.start_config
self.__plan_proposed = []
def test_server_manager(self):
# Check that we can't instanciate a client if a server isn't running
with self.assertRaises(Exception): # omniORB.CORBA._omni_sys_exc
Client()
# Check that we can instanciate a working client if a server is running
with ServerManager("../src/hppcorbaserver"):
client = Client()
client.robot.createRobot("HRP-7")
self.assertEqual(client.robot.getRobotName(), "HRP-7")
# Check that we can't instanciate a client anymore if the server was killed
with self.assertRaises(Exception): # omniORB.CORBA._omni_sys_exc
Client()
def __init__(self, context=None):
"""
Constructor
:param context: An optional string that give a name to a corba context instance
"""
self.v_max = -1 # bounds on the linear velocity for the root, negative values mean unused
self.a_max = -1 # bounds on the linear acceleration for the root, negative values mean unused
self.root_translation_bounds = [0] * 6 # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
self.root_rotation_bounds = [-3.14, 3.14, -0.01, 0.01, -0.01,
0.01] # bounds on the rotation of the root (-z, z, -y, y, -x, x)
# The rotation bounds are only used during the random sampling, they are not enforced along the path
self.extra_dof = 6 # number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
self.mu = 0.5 # friction coefficient between the robot and the environment
self.used_limbs = [] # names of the limbs that must be in contact during all the motion
self.size_foot_x = 0 # size of the feet along the x axis
self.size_foot_y = 0 # size of the feet along the y axis
self.q_init = []
self.q_goal = []
self.context = context
if context:
createContext(context)
loadServerPlugin(context, 'rbprm-corba.so')
loadServerPlugin(context, 'affordance-corba.so')
self.hpp_client = Client(context=context)
self.hpp_client.problem.selectProblem(context)
self.rbprm_client = RbprmClient(context=context)
else:
self.hpp_client = None
self.rbprm_client = None
def check_path(self, pathNumber, dt):
#check_path takes as parameter pathNumber which is the ID of the path to check and dt, the time variation(step)
# Return value
# If the path contains discontinuity or collision check_path returns the time and the path ID:
#In case of:
# -collision: it prints the bodies in collision and the time of the collision.
# -discontinuity: it prints the path ID, the time, a velocity vector(Vmax) and its index which is > than the attribut continuityThreshold
robot = self.robot
t = 0
res = False
vmax = numpy.zeros(robot.getConfigSize())
p = Client().problem.getProblem()
path_length = self.ps.pathLength(pathNumber)
r = p.robot()
while (t <= path_length):
q = numpy.array(self.ps.configAtParam(pathNumber, t))
q1 = numpy.array(self.ps.configAtParam(pathNumber, t + dt))
res, error_message = robot.isConfigValid(list(q))
vq = numpy.array(r.difference(list(q1), list(q))) / dt
if t == 0 and list(q) != list(self.q_init):
print("This path starts with a wrong configuration")
if t == path_length and list(q) != self.q_goal:
print("This path ends with a wrong configuration")
for i in range(len(vq)):
if fabs(vq[i]) > fabs(vmax[i]):
vmax[i] = fabs(vq[i])
if self.testCollision:
if res == False:
self.error_message = error_message
print("There is a collision:")
print(error_message)
print("At t = {}".format(t))
return (t, pathNumber)
if self.testContinuity:
for i, v in enumerate(vq):
if (v >= self.continuityThreshold):
print("PathId: {}\n Time: {}\n Index: {}".format(
pathNumber, t, i))
print("There is a discontinuity:\nVmax = {}".format(
vmax))
return (t, pathNumber)
t += dt
print("No collision or discontinuity in this path")
def newProblem(client=None, name=None):
if client is None:
from hpp.corbaserver import Client
client = Client()
if name is not None:
if not client.problem.selectProblem(name):
# if not created, reset it.
client.problem.resetProblem()
else:
client.problem.resetProblem()
def __init__(self, path_planner):
"""
Constructor, run the guide path and save the results
:param path_planner: an instance of a child class of AbstractPathPlanner
"""
path_planner.run()
self.path_planner = path_planner
self.path_planner.hide_rom()
# ID of the guide path used in problemSolver, default to the last one
self.pid = self.path_planner.ps.numberPaths() - 1
# Save the guide planning problem in a specific instance,
# such that we can use it again even after creating the "fullbody" problem:
self.cl = Client()
self.cl.problem.selectProblem("guide_planning")
path_planner.load_rbprm()
ProblemSolver(path_planner.rbprmBuilder)
self.cl.problem.selectProblem("default")
self.cl.problem.movePathToProblem(
self.pid, "guide_planning",
self.path_planner.rbprmBuilder.getAllJointNames()[1:])
# copy bounds and limbs used from path planning :
self.used_limbs = path_planner.used_limbs
self.root_translation_bounds = self.path_planner.root_translation_bounds
# increase bounds from path planning, to leave room for the root motion during the steps
for i in range(3):
self.root_translation_bounds[2 * i] -= 0.1
self.root_translation_bounds[2 * i + 1] += 0.1
# settings related to the 'interpolate' method:
self.dt = 0.01 # discretisation step used
self.robustness = 0 # robustness treshold
# (see https://github.com/humanoid-path-planner/hpp-centroidal-dynamics/blob/master/include/hpp/centroidal-dynamics/centroidal_dynamics.hh#L215)
self.filter_states = True # if True, after contact generation try to remove all the redundant steps
self.test_reachability = True # if True, check feasibility of the contact transition during contact generation
self.quasi_static = True # if True, use the 2-PAC method to check feasibility, if False use CROC
self.erase_previous_states = True # if False, keep^the previously computed states if 'interpolate' is called a second time
self.static_stability = True # if True, the acceleration computed by the guide is ignored during contact planning
self.init_contacts = self.used_limbs # limbs in contact in the initial configuration
# the order of the limbs in the list define the initial order in which the contacts are removed when then cannot be maintained anymore
self.end_contacts = self.used_limbs # limbs in contact in the final configuration
self.configs = [
] # will contains the whole body configuration computed after calling 'interpolate'
def test_createRobotFromPython(self):
with ServerManager("../src/hppcorbaserver"):
self.client = Client()
self.client.robot.createRobot("test")
self.assertEqual(self.client.robot.getRobotName(), "test")
types = [
"FreeFlyer",
"Planar",
"RX",
"RY",
"RZ",
"RUBX",
"RUBY",
"RUBZ",
"PX",
"PY",
"PZ",
"Translation",
]
parent = ""
for i, type in enumerate(types):
jn = "joint_{}_{}".format(type, i)
bn = "body_{}_{}".format(type, i)
self.client.robot.appendJoint(parent, jn, type,
[0, 0, 0, 0, 0, 0, 1])
self.client.robot.createSphere(bn, 0.001)
self.client.robot.addObjectToJoint(jn, bn,
[0, 0, 1, 0, 0, 0, 1])
parent = jn
robot = Robot(client=self.client)
n = len("JointModel")
_types = [
self.client.robot.getJointType(jn)[n:]
for jn in robot.jointNames
]
self.assertListEqual(types, _types)
def build_fullbody(Robot, genLimbsDB=True, context = None):
"""
Build an rbprm FullBody instance
:param Robot: The class of the robot
:param genLimbsDB: if true, generate the limbs database
:param context: An optional string that give a name to a corba context instance
:return: a fullbody instance and a problemsolver containing this fullbody
"""
# Local import, as they are optional dependencies
from hpp.corbaserver import createContext, loadServerPlugin, Client, ProblemSolver
from hpp.corbaserver.rbprm import Client as RbprmClient
if context:
createContext(context)
loadServerPlugin(context, 'rbprm-corba.so')
loadServerPlugin(context, 'affordance-corba.so')
hpp_client = Client(context=context)
hpp_client.problem.selectProblem(context)
rbprm_client = RbprmClient(context=context)
else:
hpp_client = None
rbprm_client = None
fullBody = Robot(client = hpp_client, clientRbprm = rbprm_client)
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.setJointBounds("root_joint", [-100, 100, -100, 100, -100, 100])
fullBody.client.robot.setExtraConfigSpaceBounds([-100, 100, -100, 100, -100, 100, -100, 100, -100, 100, -100, 100])
fullBody.setReferenceConfig(fullBody.referenceConfig[::] + [0] * 6)
fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6)
try:
if genLimbsDB:
fullBody.loadAllLimbs("static", nbSamples=100)
else:
fullBody.loadAllLimbs("static", nbSamples=1)
except AttributeError:
print("WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails.")
ps = ProblemSolver(fullBody)
fullBody.setCurrentConfig(fullBody.referenceConfig[::] + [0] * 6)
return fullBody, ps
def __init__(self, robotName, load=True):
Parent.__init__(self, robotName, self.rootJointType, load)
self.tf_root = "base_footprint"
self.client.basic = Client()
self.load = load
t = ps.solve()
print t
if isinstance(t, list):
t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt")
cl = Client()
cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot("toto")
ps2 = ProblemSolver(rbprmBuilder2)
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(1, "rbprm_path", rbprmBuilder.getAllJointNames())
r2 = Viewer(ps2, viewerClient=r.client)
r.client.gui.setVisibility("toto", "OFF")
#~ r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
#~ r2(q_far)
from math import sqrt
from hpp import Transform
from hpp.corbaserver.manipulation import ConstraintGraph, Constraints
from hpp.corbaserver import Client
Client ().problem.resetProblem ()
from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
vf.loadEnvironmentModel (Ground, 'ground')
vf.loadObjectModel (Pokeball, 'pokeball')
robot.setJointBounds ('pokeball/root_joint', [-.4,.4,-.4,.4,-.1,2.,
-1.0001, 1.0001,-1.0001, 1.0001,
-1.0001, 1.0001,-1.0001, 1.0001,])
q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 0, 0, 0, 1]
## Create constraint graph
graph = ConstraintGraph (robot, 'graph')
## Create constraint of relative position of the ball in the gripper when ball
## is grasped
ballInGripper = [0, .137, 0, 0.5, 0.5, -0.5, 0.5]
ps.createTransformationConstraint ('grasp', gripperName, ballName,
ballInGripper, 6*[True,])
## Create nodes and edges
# Warning the order of the nodes is important. When checking in which node
# a configuration lies, node constraints will be checked in the order of node
# creation.
graph.createNode (['grasp', 'placement'])
graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
def resetConnection(self):
self.client = Client(url= str(self.hppPlugin.getHppIIOPurl()))
self.gui = GuiClient()
def resetConnection(self):
self.client = Client(url= str(self.hppPlugin.getHppIIOPurl()),
context = str(self.hppPlugin.getHppContext()))
self.resetRobot();
self.gui = GuiClient()
def setUp(self):
self.client = Client()
#from naoqi import ALProxy
from hpp.corbaserver import Client
from hpp_corbaserver.hpp import Configuration
from nao.config import allJoints, upperJoints, legJoints, halfSitting
client = Client()
timeStep = .02
def loadRobot():
client.problem.parseFile("/home/florent/devel/nao/model/nao-hpp.kxml")
def setRobotHalfSitting():
client.robot.setCurrentConfig(0,
2 * [0.] + [0.31] + 3 * [0.] + halfSitting)
def createSmallBox():
client.obstacle.createBox('object', 0.05, 0.05, 0.05)
client.obstacle.addObstacle('object')
cfg = Configuration(trs=[.2, 0., 0.4],
rot=[1., 0., 0., 0., 1., 0., 0., 0., 1.])
client.obstacle.moveObstacleConfig('object', cfg)
def seqplay(pathId, time):
"""
Play a motion computed by KineoPathPlanner
"""
path = []
from hpp.corbaserver import Client c = Client() c.problem.interruptPathPlanning()