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()