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 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
import os from argparse import ArgumentParser from math import pi, fabs from hpp.corbaserver.manipulation import Client, ConstraintGraph, Rule, \ ConstraintGraphFactory, ProblemSolver from hpp.corbaserver.manipulation.ur5 import Robot from hpp.gepetto.manipulation import ViewerFactory from hpp.corbaserver import loadServerPlugin from hpp_idl.hpp import Equality, EqualToZero parser = ArgumentParser() parser.add_argument('-N', default=20, type=int) args = parser.parse_args() loadServerPlugin("corbaserver", "manipulation-corba.so") Client().problem.resetProblem() Robot.urdfFilename = \ "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" Robot.srdfFilename = \ "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" dir = os.getenv('PWD') class Sphere(object): rootJointType = 'freeflyer' packageName = 'hpp_environments' urdfName = 'construction_set/sphere' urdfSuffix = "" srdfSuffix = ""
from hpp.corbaserver.manipulation.robot import CorbaClient from hpp.corbaserver import loadServerPlugin from common_hpp import * # parse arguments p = argparse.ArgumentParser( description= 'Initialize estimation for the demo of Pyrene manipulating a box') p.add_argument('--ros-param', type=str, metavar='ros_param', help="The name of the ROS param containing the URDF.") args = p.parse_args() loadServerPlugin("estimation", "manipulation-corba.so") footPlacement = True projectRobotOnFloor = True comConstraint = False constantWaistYaw = True fixedArmWhenGrasping = True clients = CorbaClient(context="estimation") if not clients.manipulation.problem.selectProblem("estimation"): clients.manipulation.problem.resetProblem() robot, ps, vf, table, objects = makeRobotProblemAndViewerFactory( clients, rolling_table=True, rosParam=args.ros_param) q_neutral = robot.getCurrentConfig()
from csv import reader, writer import argparse, numpy as np from hpp import Transform from hpp.corbaserver import loadServerPlugin from hpp.corbaserver.manipulation import Constraints, ConstraintGraph, \ ProblemSolver, Rule from hpp.corbaserver.manipulation.robot import CorbaClient, HumanoidRobot from hpp.gepetto.manipulation import ViewerFactory from hpp.corbaserver.manipulation.constraint_graph_factory import \ ConstraintGraphFactory from common_hpp import createGripperLockedJoints, createLeftArmLockedJoints, \ createRightArmLockedJoints, createQuasiStaticEquilibriumConstraint, \ createWaistYawConstraint, defaultContext, shrinkJointRange loadServerPlugin(defaultContext, "manipulation-corba.so") client = CorbaClient(context=defaultContext) client.manipulation.problem.resetProblem() # parse arguments p = argparse.ArgumentParser( description='Procuce motion to calibrate arms and camera') p.add_argument('--arm', type=str, metavar='arm', default=None, help="which arm: 'right' or 'left'") p.add_argument('--N', type=int, metavar='N',
from common_hpp import * # parse arguments p = argparse.ArgumentParser (description= 'Initialize demo of Pyrene manipulating a box') p.add_argument ('--context', type=str, metavar='context', default=defaultContext, help="identifier of ProblemSolver instance") p.add_argument ('--ros-param', type=str, metavar='ros_param', help="The name of the ROS param containing the URDF.") args = p.parse_args () if args.context != defaultContext: createContext (args.context) print("context=" + args.context) loadServerPlugin (args.context, "manipulation-corba.so") isSimulation = args.context == "simulation" footPlacement = not isSimulation comConstraint = not isSimulation constantWaistYaw = not isSimulation fixedArmWhenGrasping = not isSimulation client = CorbaClient(context=args.context) newProblem(client=client.manipulation, name=args.context) robot, ps, vf, table, objects = makeRobotProblemAndViewerFactory(client, rolling_table=True, rosParam=args.ros_param) if isSimulation: ps.setMaxIterProjection (1)
from hpp.corbaserver.practicals.manipulation.ur5 import Robot from hpp.corbaserver.manipulation import ProblemSolver, Client from hpp.gepetto.manipulation import ViewerFactory from hpp.gepetto import PathPlayer from hpp.corbaserver import loadServerPlugin loadServerPlugin ("corbaserver", "manipulation-corba.so") Client ().problem.resetProblem () Robot.urdfName = "ur5_gripper" Robot.urdfSuffix = "" Robot.srdfSuffix = "" class Pokeball (object): rootJointType = 'freeflyer' packageName = 'hpp_practicals' meshPackageName = 'hpp_practicals' urdfName = 'ur_benchmark/pokeball' urdfSuffix = "" srdfSuffix = "" class Ground (object): rootJointType = 'anchor' packageName = 'hpp_practicals' urdfName = 'ur_benchmark/ground' meshPackageName = 'hpp_practicals' urdfSuffix = "" srdfSuffix = "" class Box (object): rootJointType = 'anchor' packageName = 'hpp_practicals'
# To run this test, you must launch both hppcorbaserver and roscore from hpp.corbaserver import loadServerPlugin from hpp.agimus import Client as AgimusClient import rosgraph, sys verbose = "--verbose" in sys.argv master = rosgraph.Master("") name = "hpp" loadServerPlugin('corbaserver', "agimus-hpp.so") def get_node_uri(): try: return master.lookupNode(name) except rosgraph.MasterError as e: return None cl = AgimusClient() discretization = cl.server.getDiscretization() if verbose: print("HPP uri before initialization: " + str(get_node_uri())) assert get_node_uri() == None, "HPP was not correctly disconnected from ROS" success = discretization.initializeRosNode(name, False) if verbose: print("started: " + str(success))