class HppClient(object):
def __init__ (self, withViewer = False):
self.withViewer = withViewer
self.setHppUrl()
def setHppUrl (self):
self._connect()
def _connect(self):
self.hpp = hpp.corbaserver.Client()
try:
self.manip = hpp.corbaserver.manipulation.Client ()
self.robot = hpp.corbaserver.manipulation.robot.Robot ()
self.problemSolver = hpp.corbaserver.manipulation.ProblemSolver(self.robot)
except Exception, e:
rospy.logwarn("Could not connect to manipulation server: " + str(e))
if hasattr(self, "manip"): delattr(self, "manip")
self.robot = hpp.corbaserver.robot.Robot()
self.problemSolver = hpp.corbaserver.ProblemSolver(self.robot)
rospy.loginfo("Connected to hpp")
if self.withViewer:
try:
from gepetto.corbaserver import Client as GuiClient
viewerClient = GuiClient ()
if hasattr(self, "manip"):
self.viewer = hpp.gepetto.manipulation.Viewer (self.problemSolver, viewerClient = viewerClient, displayName = self.hpp.robot.getRobotName())
else:
self.viewer = hpp.gepetto.Viewer (self.problemSolver, viewerClient = viewerClient)
rospy.loginfo("Connected to gepetto-viewer")
except Exception, e:
if hasattr(self, "viewer"): delattr(self, "viewer")
rospy.logwarn("Could not connect to gepetto-viewer: " + str(e))
def __init__ (self, problemSolver, viewerClient = None):
self.problemSolver = problemSolver
self.robot = problemSolver.robot
shouldLoadRobot = False
if not viewerClient:
shouldLoadRobot = True
viewerClient = GuiClient ()
self.createWindowAndScene (viewerClient, "hpp_")
self.client = viewerClient
self.displayName = self.robot.displayName
if hasattr (self.robot, 'meshPackageName'):
meshPackageName = self.robot.meshPackageName
else:
meshPackageName = self.robot.packageName
# Load robot in viewer
self.buildRobotBodies ()
if shouldLoadRobot:
rospack = rospkg.RosPack()
packagePath = rospack.get_path (self.robot.packageName)
meshPackagePath = rospack.get_path (meshPackageName)
dataRootDir = os.path.dirname (meshPackagePath) + "/"
packagePath += '/urdf/' + self.robot.urdfName + \
self.robot.urdfSuffix + '.urdf'
self.client.gui.addURDF (self.displayName, packagePath, dataRootDir)
self.client.gui.addToGroup (self.displayName, self.sceneName)
def createViewer(self,
ViewerClass=Viewer,
viewerClient=None,
ghost=False,
host=None,
*args,
**kwargs):
if host is not None and viewerClient is None:
from gepetto.corbaserver import Client as GuiClient
try:
viewerClient = GuiClient(host=host)
except Exception as e:
if ghost:
print("Failed to connect to the viewer.")
print("Check whether gepetto-gui is properly started.")
viewerClient = _GhostViewerClient()
else:
raise e
v = ViewerClass(self.problemSolver,
viewerClient,
ghost=ghost,
*args,
**kwargs)
v.removeLightSources = self.removeLightSources
for call in self.guiRequest:
kwargs = call[1].copy()
s = kwargs.pop('self')
f = call[0]
f(v, **kwargs)
return v
def __init__(self,
problemSolver,
viewerClient=None,
collisionURDF=False,
displayName=None,
displayArrows=False,
displayCoM=False):
from gepetto.corbaserver import Client as GuiClient
self.problemSolver = problemSolver
self.robot = problemSolver.robot
self.collisionURDF = collisionURDF
self.color = Color()
if not viewerClient:
viewerClient = GuiClient()
self.createWindowAndScene(viewerClient, "hpp_")
self.client = viewerClient
self.callbacks = []
if displayName is not None:
self.displayName = displayName
else:
self.displayName = self.robot.displayName
# Load robot in viewer
self.buildRobotBodies()
self._initDisplay()
# create velocity and acceleration arrows :
self.displayArrows = displayArrows
if displayArrows:
if self.robot.client.robot.getDimensionExtraConfigSpace() < 6:
raise RuntimeError(
"displayArrows can only be True if the robot have at least 6 extraDof storing velocity and acceleration of the root."
)
self.colorVelocity = [0.2, 1, 0, 0.6]
self.colorAcceleration = [1, 0, 0, 0.6]
self.arrowRadius = 0.01
self.arrowMinSize = 0.05
self.arrowMaxSize = 1. - self.arrowMinSize
if (not ("Vec_Velocity" in self.client.gui.getNodeList())):
self.client.gui.addArrow("Vec_Velocity", self.arrowRadius,
self.arrowMinSize, self.colorVelocity)
self.client.gui.addToGroup("Vec_Velocity", self.sceneName)
self.client.gui.setVisibility("Vec_Velocity", "OFF")
self.client.gui.addArrow("Vec_Acceleration", self.arrowRadius,
self.arrowMinSize,
self.colorAcceleration)
self.client.gui.addToGroup("Vec_Acceleration", self.sceneName)
self.client.gui.setVisibility("Vec_Acceleration", "OFF")
self.amax = omniORB.any.from_any(
self.problemSolver.client.problem.getParameter(
"Kinodynamic/accelerationBound"))
self.vmax = omniORB.any.from_any(
self.problemSolver.client.problem.getParameter(
"Kinodynamic/velocityBound"))
self.displayCoM = displayCoM
def __init__(self, problem, viewerClient=None):
# self.rod = flectoClient().rod
self.problemSolver = problem
self.robot = problem.robot
if not viewerClient:
viewerClient = GuiClient()
self.createWindowAndScene(viewerClient, "hpp_")
self.client = viewerClient
self.displayName = self.robot.displayName
self.buildRobotBodies()
self.client.gui.addToGroup(self.displayName, self.sceneName)
def createViewer(self,
ViewerClass=Viewer,
viewerClient=None,
host=None,
collisionURDF=False):
if host is not None and viewerClient is None:
from gepetto.corbaserver import Client as GuiClient
viewerClient = GuiClient(host=host)
v = Parent.createViewer(self, ViewerClass, viewerClient, host,
collisionURDF)
return v
def createViewer(self,
ViewerClass=Viewer,
viewerClient=None,
host=None,
collisionURDF=False):
if host is not None and viewerClient is None:
from gepetto.corbaserver import Client as GuiClient
viewerClient = GuiClient(host=host)
v = ViewerClass(self.problemSolver, viewerClient, collisionURDF)
for call in self.guiRequest:
kwargs = call[1].copy()
s = kwargs.pop('self')
f = call[0]
f(v, **kwargs)
return v
from hpp.corbaserver.pr2 import Robot
robot = Robot('pr2', False)
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
from gepetto.corbaserver import Client as GuiClient
guiClient = GuiClient()
from hpp.gepetto import Viewer
Viewer.sceneName = '0_scene_hpp_'
r = Viewer(ps, guiClient)
from hpp.gepetto import PathPlayer
pp = PathPlayer(robot.client, r)
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()