def initScene(Robot, envName="multicontact/ground", genLimbsDB=True):
from hpp.gepetto import Viewer, ViewerFactory
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver import ProblemSolver
fullBody = Robot()
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)
vf = ViewerFactory(ps)
vf.loadObstacleModel(
"package://hpp_environments/urdf/" + envName + ".urdf", "planning")
v = vf.createViewer(ghost=True, displayCoM=True)
v(fullBody.getCurrentConfig())
return fullBody, v
def initScene(Robot, envName="multicontact/ground"):
from hpp.gepetto import Viewer, ViewerFactory
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver import ProblemSolver
fullBody = Robot()
fullBody.client.robot.setDimensionExtraConfigSpace(6)
try:
fullBody.loadAllLimbs("static", nbSamples=1)
except AttributeError:
print "WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails."
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp_environments", envName, "planning")
v = vf.createViewer(displayCoM=True)
v(fullBody.getCurrentConfig())
return fullBody, v
def initScene(Robot,
envName="multicontact/ground",
genLimbsDB=True,
context=None):
"""
Create and initialize a hpp.gepetto.Viewer object with a hpp.corbaserver.rbprm.FullBody object
:param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot)
:param envName: the name of the environment file to load. Load a flat floor at z = 0 by default
Currently only work with environments from the hpp-environment package
:param genLimbsDB: If True (default value) generate the limbs database of the fullbody object
:param context: An optional string that give a name to a corba context instance
:return: a Fullbody object and a Viewer object
"""
from hpp.gepetto import ViewerFactory
fullBody, ps = build_fullbody(Robot, genLimbsDB, context)
vf = ViewerFactory(ps)
vf.loadObstacleModel(
"package://hpp_environments/urdf/" + envName + ".urdf", "planning")
v = vf.createViewer(ghost=True, displayCoM=True)
v(fullBody.getCurrentConfig())
return fullBody, v
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:3] = [6.5, -4, 0.4] # root position
q_init[3:7] = [0, 0, 0, 1] # root rotation
# set initial velocity (along x,y,z axis) :
q_init[-6:-3] = [0, 0, 0]
# q_goal[0:3] = [6.5,-1,0.4] # straight line
q_goal[0:3] = [3, -4, 0.4] # easy goal position
# q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position
# set goal velocity (along x,y,z axis) :
q_goal[-6:-3] = [0, 0, 0]
vf.loadObstacleModel("iai_maps", "room", "room")
# with displayArrow parameter the viewer will display velocity and acceleration of the
# center of the robot with 3D arrow. The length scale with the amplitude with a length
# of 1 for the maximal amplitude.
v = vf.createViewer(displayArrows=True)
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
# select kinodynamic methods :
ps.selectSteeringMethod("Kinodynamic")
ps.selectDistance("Kinodynamic")
# the Kinodynamic steering method require a planner that build directionnal roadmap
# (with oriented edges) as the trajectories cannot always be reverse.
ps.selectPathPlanner("BiRRTPlanner")
from hpp.corbaserver.fleche import Robot
robot = Robot('fleche')
robot.setJointBounds("root_joint", [-70, 4.375, -5, 4.375, -5, 7])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:2] = [-60, -4]
vf.loadObstacleModel("iai_maps", "ville1", "ville1_link")
v = vf.createViewer()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
print(ps.solve())
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.displayPath(1)
#pp (0)
import json
a = ps.getWaypoints(1)
class Platform():
# main_agent = None
agents = []
# problem solver
ps = None
# path player
pp = None
# view factory
vf = None
# viewer
r = None
env = None
# a dictionary to get the agent's index
index_dic = {}
#for tree searching
tree = None # the root of the three
current_node = None
# pp = PathPlayer (rbprmBuilder.client.basic,ls r)
def __init__(self, agents):
self.agents = agents
for i in range(len(agents)):
a = agents[i]
self.index_dic[agents[i].robot.name] = i
self.agents[i].registerPlatform(self, i)
print 'the agent ', agents[
i].robot.name, ' is now registered with the index ', self.getInidex(
agents[i].robot.name)
self.tree = self.getStartNode()
self.current_node = self.tree
def getStartNode(self):
init_configs = []
for a in self.agents:
init_configs.append(a.start_config)
return Node(init_configs)
def start(self):
# self.problem.selectProblem(0)
self.ps = ProblemSolver(self.agents[0].robot)
self.vf = ViewerFactory(self.ps)
if self.env != None:
self.vf.loadObstacleModel(self.env.packageName, self.env.urdfName,
self.env.name)
self.r = self.vf.createViewer()
for a in self.agents:
a.startDefaultSolver()
a.setBounds()
a.setEnvironment()
a.solve()
a.storePath()
# self.loadAgentView(a.index)
# self.r(a.start_config)
print 'the agent ', a.robot.name, ' now has a backup plan of length', a.getProposedPlanLength(
)
# self.pp = PathPlayer (self.agents[0], self.r)
def loadAgentView(self, index, default=False): #default position or not
self.ps = self.agents[index - 1].ps
self.vf = ViewerFactory(self.ps)
# self.vf.loadObstacleModel(self.env.packageName, self.env.urdfName, self.env.name)
self.r = self.vf.createViewer()
# print '---------------->', len(self.agents[index - 1].init_config)
if default:
self.r(self.agents[index - 1].current_config)
# self.r.computeObjectPosition()
def getInidex(self, robot_name):
return self.index_dic[robot_name]
def setEnvironment(self, env):
self.env = env
# self.ps.moveObstacle('airbase_link_0', [0,0, -3, 1,0,0,0])
# self.r = self.vf.createViewer()
# def startViewer(self):
# self.r = vf.createViewer()
def updateViewAtTime(self, t):
config = []
for a in self.agents:
config.append(a.getConfigOfProposedPlanAtTime(t))
self.r(config)
def playAllProposedPath(self):
print 'play proposed path'
max_time = 0
for a in self.agents:
l = a.getProposedPlanLength()
if l > max_time:
max_time = l
for t in range(max_time):
# print 'time is ', t
for i in range(len(self.agents)):
a = self.agents[i]
if a.getProposedPlanLength() > t:
# print 'agent ', a.index,
self.loadAgentView(i + 1)
# and then set the agent to its current configuration
self.r(a.getConfigOfProposedPlanAtTime(t))
# sleep(0.003)
def playAllPermittedPath(self):
max_time = 0
for a in self.agents:
l = a.getPermittedPlanLength()
if l > max_time:
max_time = l
for t in range(max_time):
# print 'time is ', t
for i in range(len(self.agents)):
a = self.agents[i]
if a.getPermittedPlanLength() > t:
# print 'agent ', a.index,
self.loadAgentView(i + 1)
# and then set the agent to its current configuration
self.r(a.getConfigOfPermittedPlanAtTime(t))
# sleep(0.003)
def validateAllPaths(self, agents_remained):
print '******* start validation **********'
# print agents_remained
max_time = 0
for i in agents_remained:
a = self.agents[i]
a.startDefaultSolver()
a.setBounds()
a.setEnvironment()
a.loadOtherAgents()
l = a.getProposedPlanLength()
if l > max_time:
max_time = l
for t in range(max_time):
print '\n this is time ', t
for i in agents_remained:
a = self.agents[i]
a.startDefaultSolver()
a.setBounds()
a.setEnvironment()
a.loadOtherAgents()
# print 'this is robot ', a.robot.name
# a1.obstacle.getObstacleNames(False, 1000)
if a.getProposedPlanLength() > t:
# myconfig = a.getConfigOfProposedPlanAtTime(t)
# myspec = a.getMoveSpecification(myconfig)
# print 'the agent is at ', myspec[0], myspec[1]
# first of all, move all the obstacles
for oa in self.agents: # other agents
if a.index != oa.index:
# print '\t and moving the ghost of ', oa.robot.name
if not (oa.index in agents_remained
) or oa.getProposedPlanLength() <= t:
config = oa.end_config
else:
config = oa.getConfigOfProposedPlanAtTime(t)
spec = oa.getMoveSpecification(config)
a.obstacle.moveObstacle(
oa.robot.name + 'base_link_0', spec)
print '\tmove ghost', oa.robot.name, ' to ', spec[
0], spec[1]
# secondly, test if the configuration is valid
(result, _) = a.robot.isConfigValid(
a.getConfigOfProposedPlanAtTime(t))
if not result:
return t
# if everything is fine at each time slot, return -1
return -1
def construct_tree(self, iteration):
print '******************* this is iternation ', iteration, ' ***********************'
self.current_node.printInformation()
if iteration > 0:
#expand the tree by doing planning for each agent and find the collision momment
for i in self.current_node.getAgentsRemained():
a = self.agents[i]
print '>>>>>>>>>>>>this is agent', a.robot.name, ' computing '
if (a.computePlan(self.current_node) == -1):
print 'the agent is now using its backup plan/plan from last time'
# line = input()
# self.playAllProposedPath()
t = self.validateAllPaths(self.current_node.getAgentsRemained())
print 'in this iteration, the collision appears at time ', t
if t == -1: # the path is valid! we terminate the process
paths = []
indexes_and_paths = []
for i in self.current_node.agent_remained:
a = self.agents[i]
indexes_and_paths.append((a.index, a.obtainProposedPlan()))
child = self.current_node.expand(indexes_and_paths, [])
return (True, child.paths)
else:
reached = []
indexes_and_paths = []
for i in self.current_node.getAgentsRemained():
a = self.agents[i]
if a.getProposedPlanLength(
) > t - 1: # up to t, because t is the moment of collision!
path = a.obtainProposedPlan()[:t - 1]
indexes_and_paths.append((i, path))
else:
reached.append(
i
) # reached, therefore remove from the remaining list
indexes_and_paths.append((i, a.obtainProposedPlan()))
self.current_node = self.current_node.expand(
indexes_and_paths, reached)
return self.construct_tree(iteration - 1)
else: # can not find a path for each agent within limited iteration
return (False, None)
# remove those who has already got to where they suppose to be
# at least one step ----------- frangment
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.selectPathValidation ("Dichotomy", 0)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
from talos_rbprm.talos import Robot
from hpp.gepetto import Viewer
import time
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
import time
fullBody = Robot()
fullBody.setJointBounds("root_joint", [-5, 5, -1.5, 1.5, 0.95, 1.05])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds([0] * 12)
ps = ProblemSolver(fullBody)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp_environments", "multicontact/table_140_70_73",
"planning")
q_ref = [
0.0,
0.0,
1.0232773,
0.0,
0.0,
0.0,
1, #Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, #Left Leg
robot = Robot ('ur5')
cl = robot.client
ps = ProblemSolver (robot)
# q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF#
q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [0, -1.57, 1.57, 3.267256451, 0, 0]
q_init = q1;
# q_goal = q2
q_goal = q3
from hpp.gepetto import ViewerFactory, PathPlayerGui
vf = ViewerFactory (ps)
# vf.loadObstacleModel ("ur_description","obstacles","obstacles")
vf.loadObstacleModel ("ur_description","table","table")
# vf.loadObstacleModel ("ur_description","wall","wall")
vf(q1)
ps.lockJoint("elbow_joint", [q1[2]])
ps.selectPathValidation("Progressive", .05)
ps.setParameter("SplineGradientBased/alphaInit", CORBA.Any(CORBA.TC_double, 0.3))
ps.setParameter("SplineGradientBased/alwaysStopAtFirst", CORBA.Any(CORBA.TC_boolean, True))
ps.setParameter("SplineGradientBased/linearizeAtEachStep", CORBA.Any(CORBA.TC_boolean, False))
print "Optimizer parameters are:"
print "alphaInit:", ps.getParameter("SplineGradientBased/alphaInit").value()
print "alwaysStopAtFirst:", ps.getParameter("SplineGradientBased/alwaysStopAtFirst").value()
print "linearizeAtEachStep:", ps.getParameter("SplineGradientBased/linearizeAtEachStep").value()
from hpp.corbaserver.practicals.ur5 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import ViewerFactory, PathPlayer
robot = Robot ('ur5')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
vf.loadObstacleModel ("hpp_practicals","ur_benchmark/obstacles","obstacles")
vf.loadObstacleModel ("hpp_practicals","ur_benchmark/table","table")
vf.loadObstacleModel ("hpp_practicals","ur_benchmark/wall","wall")
v = vf.createViewer ()
q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [1.57, -1.57, -1.8, 0, 0.8, 0]
v (q2)
v (q3)
ps.setInitialConfig (q2)
ps.addGoalConfig (q3)
from motion_planner import MotionPlanner
m = MotionPlanner (robot, ps)
pathId = m.solveBiRRT (maxIter = 1000)
pp = PathPlayer (v)
#pp (pathId)
from hpp.environments import Buggy
robot = Buggy("buggy")
robot.setJointBounds ("base_joint_xy", [-5, 16, -4.5, 4.5])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
gui = ViewerFactory (ps)
gui.loadObstacleModel ('hpp_environments', "scene", "scene")
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init[0:2] = [-3.7, -4];
gui (q_init)
q_goal [0:2] = [15,2]
gui (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectSteeringMethod ("ReedsShepp")
ps.selectPathPlanner ("DiffusingPlanner")
ps.addPathOptimizer ("RandomShortcut")
t = ps.solve ()
print ("solving time", t)
gameWidget.update()
def end():
gameWidget.end()
sys.argv = []
gui = ViewerClient()
gui.gui.createWindow("ia")
gui.gui.createWindow("player")
iaRobot = Buggy("buggy_ia")
ips = ProblemSolver(iaRobot)
ivf = ViewerFactory(ips)
ivf.loadObstacleModel('hpp_environments', "scene", "scene_ia")
iv = ivf.createViewer(viewerClient=gui)
iaRobot.client.problem.selectProblem("player")
playerRobot = Buggy("buggy_player")
pps = ProblemSolver(playerRobot)
pvf = ViewerFactory(pps)
pvf.loadObstacleModel('hpp_environments', "scene", "scene_player")
pvf(playerRobot.client.robot.getCurrentConfig())
pv = pvf.createViewer(viewerClient=gui)
gui.gui.addSceneToWindow("buggy_player", 1)
gui.gui.addSceneToWindow("scene_player", 1)
gui.gui.addSceneToWindow("buggy_ia", 0)
gui.gui.addSceneToWindow("scene_ia", 0)
# To launch the game
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:3] = [6.5,-4,0.4] #root position
q_init[3:7] = [0,0,0,1] #root rotation
#set initial velocity (along x,y,z axis) :
q_init[-6:-3]=[0,0,0]
#q_goal[0:3] = [6.5,-1,0.4] # straight line
q_goal [0:3] = [3,-4,0.4] # easy goal position
#q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position
#set goal velocity (along x,y,z axis) :
q_goal[-6:-3]=[0,0,0]
vf.loadObstacleModel ("iai_maps", "room", "room")
# with displayArrow parameter the viewer will display velocity and acceleration of the center of the robot with 3D arrow. The length scale with the amplitude with a length of 1 for the maximal amplitude
v = vf.createViewer(displayArrows = True)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.addPathOptimizer ("RandomShortcut")
#select kinodynamic methods :
ps.selectSteeringMethod("Kinodynamic")
ps.selectDistance("Kinodynamic")
# the Kinodynamic steering method require a planner that build directionnal roadmap (with oriented edges) as the trajectories cannot always be reversed.
ps.selectPathPlanner("BiRRTPlanner")
print (ps.solve ())
from hpp.environments import Buggy
from hpp.gepetto import PathPlayer
robot = Buggy("buggy")
robot.setJointBounds ("root_joint", [-5, 16, -4.5, 4.5,
-1.01, 1.01, -1.01, 1.01])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
gui = ViewerFactory (ps)
gui.loadObstacleModel ('hpp_environments', "scene", "scene")
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init[0:2] = [-3.7, -4];
gui (q_init)
q_goal [0:2] = [15,2]
gui (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectSteeringMethod ("ReedsShepp")
ps.selectPathPlanner ("DiffusingPlanner")
ps.addPathOptimizer ("RandomShortcut")
t = ps.solve ()
print ("solving time", t)
q_goal = q_init[::]
q_init[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['torso_lift_joint']
q_init[rank] = 0.2
q_goal[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['l_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['l_elbow_flex_joint']
q_goal[rank] = -0.5
rank = robot.rankInConfiguration['r_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['r_elbow_flex_joint']
q_goal[rank] = -0.5
vf.loadObstacleModel("package://hpp_tutorial/urdf/kitchen_area.urdf",
"kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
# print (ps.solve ())
## Uncomment this to connect to a viewer server and play solution paths
#
# v = vf.createViewer()
# from hpp.gepetto import PathPlayer
# pp = PathPlayer (v)
# pp (0)
ps = ProblemSolver(robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:3] = [0, 0, 0]
vf(q_init)
q_goal[0:7] = [10, 6, 0.11, 0.707460067, 0.706753314, 0, 0]
q_goal[0:7] = [9.7, 6, 0.11, 0.707460067, 0.706753314, 0, 0]
vf(q_goal)
vf.loadObstacleModel("iai_maps", "env_complexe", "simple")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.clearRoadmap()
ps.selectPathPlanner("interactive")
white = [1.0, 1.0, 1.0, 1.0]
brown = [0.85, 0.75, 0.15, 0.5]
black = [0, 0, 0, 1]
r = vf.createViewer()
ps.addPathOptimizer("RandomShortcut")
#print r.solveAndDisplay("rm1",1,white,0.00,1,black)
from hpp.corbaserver.practicals.ur5 import Robot
from hpp.corbaserver import ProblemSolver, Client
from hpp.gepetto import ViewerFactory, PathPlayer
Client().problem.resetProblem()
import readline
import rlcompleter
readline.parse_and_bind("tab: complete")
robot = Robot('ur5')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadObstacleModel(
"package://hpp_practicals/urdf/ur_benchmark/obstacles.urdf", "obstacles")
vf.loadObstacleModel("package://hpp_practicals/urdf/ur_benchmark/table.urdf",
"table")
vf.loadObstacleModel("package://hpp_practicals/urdf/ur_benchmark/wall.urdf",
"wall")
q1 = [0, -1.57, 1.57, 0, 0, 0]
q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [1.57, -1.57, -1.8, 0, 0.8, 0]
ps.setInitialConfig(q2)
ps.addGoalConfig(q3)
from motion_planner import MotionPlanner
m = MotionPlanner(robot, ps)
pathId = m.solveBiRRT(maxIter=1000)
class HppServerInitializer(HppClient):
subscribersDict = {
"hpp": {
"reset": [Bool, "reset"],
"create_viewer": [Empty, "_createViewer"],
"configure": [Empty, "configure"],
"load_environment": [String, "loadEnvironment"],
},
}
def __init__(self):
super(HppServerInitializer, self).__init__()
self.subscribers = self._createTopics("", self.subscribersDict, True)
self.setHppUrl()
self.robot = None
self.ps = None
self.tfListener = TransformListener()
self.world_frame = "/world"
def reset(self, msg):
self.initialize()
self.loadRobot()
if msg.data:
self.loadEnvironment(param="/environment/description",
paramName="/environment/name")
self.configure()
try:
self.createViewer()
except:
rospy.loginfo("Could not reach the Gepetto-viewer")
def initialize(self):
self.setHppUrl()
hpp = self._hpp()
# Reset the problem if necessary
if self.robot is not None:
hpp.problem.resetProblem()
self.robot = None
self.ps = None
self.vf = None
def loadRobot(self):
hppClient = self._hpp()
# Load the robot
robotName = rospy.get_param("/robot_name", "robot")
rootJointType = rospy.get_param("/robot_root_joint_type", "anchor")
urdfString = rospy.get_param("/robot_description")
srdfString = rospy.get_param("/robot_description_semantic", "")
hppClient.robot.loadRobotModelFromString(robotName, rootJointType,
urdfString, srdfString)
self.robot = hpp.corbaserver.robot.Robot(robotName,
rootJointType,
client=hppClient,
load=False)
self.ps = hpp.corbaserver.ProblemSolver(self.robot)
self.vf = ViewerFactory(self.ps)
self.vf.loadRobot(urdfString)
# Set the root joint position in the world.
base = self.robot.getLinkNames("root_joint")[0]
p, q = self.tfListener.lookupTransform(self.world_frame, base,
rospy.Time(0))
self.robot.setJointPosition("root_joint", p + q)
def loadEnvironment(self,
param=None,
xmlString=None,
paramName=None,
name=None):
if name is None:
name = rospy.get_param(
paramName) if paramName is not None else "obstacle"
if param is not None:
self.vf.loadObstacleModel("", rospy.get_param(param), name)
elif xmlString is not None:
self.vf.loadObstacleModel("", xmlString, name)
def configure(self):
hpp = self._hpp()
# Setup the planner
setParameters(hpp, "", rospy.get_param("/hpp/parameters", dict()))
if rospy.has_param("/hpp/path_validation/method"):
ps.selectPathValidation(
rospy.get_param("/hpp/path_validation/method"),
rospy.get_param("/hpp/path_validation/tolerance", 0.05))
self.ps.clearPathOptimizers()
if rospy.has_param("/hpp/path_optimizers"):
pathOpts = rospy.get_param("/hpp/path_optimizers",
["RandomShortcut"])
if isinstance(pathOpts, list):
for n in pathOpts:
self.ps.addPathOptimizer(n)
else:
rospy.logerr(
"Parameter /hpp/path_optimizers shoud be a list of strings."
)
def _createViewer(self, msg):
self.createViewer()
def createViewer(self, *args, **kwargs):
host = rospy.get_param("/gepetto_viewer/host", "localhost")
try:
return self.vf.createViewer(host=host, *args, **kwargs)
except:
rospy.loginfo("Could not reach the Gepetto-viewer")
from math import sqrt
from hpp.corbaserver.tom import Robot
from hpp.gepetto import PathPlayer
#Robot.urdfName = "tom_full"
robot = Robot ('tom')
robot.setJointBounds ('base_joint_xy', [-1,1,-1,1])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
#r.loadObstacleModel ("tom_description", "table", "table")
vf.loadObstacleModel ("hpp_tutorial", "box", "box")
box_position = [.5,0,.5,1,0,0,0]
table_position = (-0.90, -0.91, -0.42, .5, .5, .5, .5)
#r.moveObstacle ("table/table_tom_link_0", table_position)
#r.moveObstacle ("tom/box_tom_link_0", table_position)
vf.moveObstacle ("box/base_link_0", box_position)
q_init = robot.halfSitting [::]
dict_goal = {
'l_shoulder_pan_joint': -1.62236446982,
'l_shoulder_lift_joint': -0.741943917854,
'l_elbow_joint': 0.952500010443,
'l_wrist_1_joint': -1.27920005559,
'l_wrist_2_joint': -2.22347756642,
q_goal = q_init[::]
q_init[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['torso_lift_joint']
q_init[rank] = 0.2
q_goal[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['l_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['l_elbow_flex_joint']
q_goal[rank] = -0.5
rank = robot.rankInConfiguration['r_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['r_elbow_flex_joint']
q_goal[rank] = -0.5
vf.loadObstacleModel("package://iai_maps/urdf/kitchen_area.urdf", "kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
# print (ps.solve ())
## Uncomment this to connect to a viewer server and play solution paths
#
# v = vf.createViewer()
# from hpp.gepetto import PathPlayer
# pp = PathPlayer (v)
# pp (0)
def play():
gameWidget.update()
def end():
gameWidget.end()
sys.argv = []
gui = ViewerClient()
gui.gui.createWindow("ia")
gui.gui.createWindow("player")
iaRobot = Buggy("buggy_ia")
ips = ProblemSolver(iaRobot)
ivf = ViewerFactory(ips)
ivf.loadObstacleModel('hpp_environments', "scene", "scene_ia")
iv = ivf.createViewer(viewerClient = gui)
iaRobot.client.problem.selectProblem("player")
playerRobot = Buggy("buggy_player")
pps = ProblemSolver(playerRobot)
pvf = ViewerFactory(pps)
pvf.loadObstacleModel('hpp_environments', "scene", "scene_player")
pvf(playerRobot.client.robot.getCurrentConfig())
pv = pvf.createViewer(viewerClient = gui)
gui.gui.addSceneToWindow("buggy_player", 1)
gui.gui.addSceneToWindow("scene_player", 1)
gui.gui.addSceneToWindow("buggy_ia", 0)
gui.gui.addSceneToWindow("scene_ia", 0)
# To launch the game
q_init[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['torso_lift_joint']
q_init[rank] = 0.2
r(q_init)
q_goal[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['l_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['l_elbow_flex_joint']
q_goal[rank] = -0.5
rank = robot.rankInConfiguration['r_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['r_elbow_flex_joint']
q_goal[rank] = -0.5
r(q_goal)
r.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.selectPathPlanner("PRM")
ps.addPathOptimizer("RandomShortcut")
# ps.solve ()
# from hpp.gepetto import PathPlayer
# pp = PathPlayer (robot.client, r)
# pp (0)
# pp (1)
from hpp.quadcopter import Robot
robot = Robot("quad")
robot.client.robot.setDimensionExtraConfigSpace(9)
# robot.client.robot.setDimensionExtraConfigSpace(6)
robot.setJointBounds ("base_joint_xyz", [-5, 16, -4.5, 4.5, 0, 0.6])
# robot.client.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-1,1,-1,1,-1,1,-1,1])
robot.client.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory, PathPlayer
vf = ViewerFactory (ps)
vf.loadObstacleModel ('hpp-quadcopter', "scene", "scene")
q_init = robot.getCurrentConfig ()
q_init[2] = 0.5
q_goal = q_init [::]
q_init [0:2] = [-3.7, -4];
vf (q_init)
q_goal [0:2] = [15,2]
vf (q_goal)
q = q_init[:]
q[0:2] = [2,0]
vf(q)
#~ ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")
from hpp.corbaserver.practicals.ur5 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import ViewerFactory, PathPlayer
robot = Robot('ur5')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/obstacles", "obstacles")
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/table", "table")
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/wall", "wall")
v = vf.createViewer()
q1 = [0, -1.57, 1.57, 0, 0, 0]
q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [1.57, -1.57, -1.8, 0, 0.8, 0]
v(q2)
v(q3)
ps.setInitialConfig(q2)
ps.addGoalConfig(q3)
from motion_planner import MotionPlanner
m = MotionPlanner(robot, ps)
pathId = m.solveBiRRT(maxIter=1000)
pp = PathPlayer(v)
packageName = "talos_data"
meshPackageName = "talos_data"
rootJointType = "freeflyer"
urdfName = "talos"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("root_joint", [-5, 5, -1.5, 1.5, 0.95, 1.05])
ps = ProblemSolver(fullBody)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp-rbprm-corba", "table_140_70_73", "planning")
q_ref = [
0.0,
0.0,
1.0232773,
0.0,
0.0,
0.0,
1, #Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, #Left Leg
