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