def makeRobotProblemAndViewerFactory(clients, rolling_table=True):
objects = list()
robot = HumanoidRobot("talos", "talos", rootJointType="freeflyer", client=clients)
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-2, 2, -2, 2, 0, 2])
shrinkJointRange (robot, 0.95)
ps = ProblemSolver(robot)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("EnforceTransitionSemantic")
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory(ps)
objects.append(Box(name="box", vf=vf))
robot.setJointBounds("box/root_joint", [-2, 2, -2, 2, 0, 2])
# Loaded as an object to get the visual tags at the right position.
# vf.loadEnvironmentModel (Table, 'table')
if rolling_table:
table = RollingTable(name="table", vf=vf)
else:
table = Table(name="table", vf=vf)
robot.setJointBounds("table/root_joint", [-2, 2, -2, 2, -2, 2])
return robot, ps, vf, table, objects
def initRobot(self):
# The first robot is loaded differently by hpp
robot_id = len(self.robots) + int(self.robot is not None)
robot_name = "talos_" + str(robot_id)
if self.robot is None:
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
self.robot = Robot(
"talos", robot_name, rootJointType="freeflyer", client=self.client
)
self.ps = ProblemSolver(self.robot)
self.ps.setErrorThreshold(0.00000001)
self.ps.setMaxIterProjection(100)
self.vf = ViewerFactory(self.ps)
else:
self.robots.append(OtherRobot(name=robot_name, vf=self.vf))
self.robot.setJointBounds(robot_name + "/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
rank, size = self.getRankAndConfigSize(robot_id)
self.q[rank : rank + size] = self.robot.getCurrentConfig()[rank : rank + size]
return robot_id
def makeRobotProblemAndViewerFactory(corbaclient):
robot = Robot("dev",
"talos",
rootJointType="freeflyer",
client=corbaclient)
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver(robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory(ps)
vf.loadObjectModel(Object, "box")
robot.setJointBounds("box/root_joint", [-1, 1, -1, 1, 0, 2])
vf.loadEnvironmentModel(Table, "table")
return robot, ps, vf
def makeRobotProblemAndViewerFactory(clients,
rolling_table=True,
rosParam=None):
if rosParam is not None:
import rospy, os, hpp
HumanoidRobot.urdfString = rospy.get_param(rosParam)
srdfFilename = hpp.retrieveRosResource(HumanoidRobot.srdfFilename)
with open(srdfFilename, 'r') as f:
HumanoidRobot.srdfString = f.read()
objects = list()
robot = HumanoidRobot("talos",
"talos",
rootJointType="freeflyer",
client=clients)
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-2, 2, -2, 2, 0, 2])
shrinkJointRange(robot, 0.95)
ps = ProblemSolver(robot)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("EnforceTransitionSemantic")
ps.addPathOptimizer("SimpleTimeParameterization")
ps.selectPathValidation('Graph-Progressive', 0.01)
vf = ViewerFactory(ps)
objects.append(Box(name="box", vf=vf))
robot.setJointBounds("box/root_joint", [-2, 2, -2, 2, 0, 2])
# Loaded as an object to get the visual tags at the right position.
# vf.loadEnvironmentModel (Table, 'table')
if rolling_table:
table = RollingTable(name="table", vf=vf)
else:
table = Table(name="table", vf=vf)
robot.setJointBounds("table/root_joint", [-2, 2, -2, 2, -2, 2])
return robot, ps, vf, table, objects
urdfSuffix = ""
srdfSuffix = ""
class Ground(object):
rootJointType = 'anchor'
packageName = 'hpp_environments'
urdfName = 'construction_set/ground'
urdfSuffix = ""
srdfSuffix = ""
nSphere = 2
robot = Robot('ur3-spheres', 'ur3')
ps = ProblemSolver(robot)
ps.setErrorThreshold(1e-4)
ps.setMaxIterProjection(40)
vf = ViewerFactory(ps)
# Change bounds of robots to increase workspace and avoid some collisions
robot.setJointBounds('ur3/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds('ur3/shoulder_lift_joint', [-pi, 0])
robot.setJointBounds('ur3/elbow_joint', [-2.6, 2.6])
vf.loadEnvironmentModel(Ground, 'ground')
objects = list()
p = ps.client.basic.problem.getProblem()
r = p.robot()
# ILLC, University of Amsterdam, The Netherlands # [email protected] # hpp-manipulation-server # gepetto-viewer-server from hpp.gepetto.manipulation import ViewerFactory from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph from ManiHyQ import HyQ as MetaHyQ # the manipulation robot is the meta robot we use to add other robots on from HyQ import HyQ # this is the class of other robots we add to the manipulation robot from math import cos, sin, asin, acos, atan2, pi from time import sleep import sys meta = MetaHyQ('r0') ps = ProblemSolver(meta) fk = ViewerFactory(ps) meta.setJointBounds("r0/base_joint_xy", [-30, 30, -30, 30]) name_of_scene = "simple_boeing" def loadOtherRobots(amount): # only load 2, 3, etc print 'there are ', amount, ' robots' for i in range(amount - 1): fk.loadObjectModel(HyQ, 'r' + str(i + 1)) print 'loading', 'r' + str(i + 1) meta.setJointBounds('r' + str(i + 1) + "/base_joint_xy", [-30, 30, -30, 30]) print 'DOF: ', len(meta.getCurrentConfig())
