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 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
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()
for i in range(nSphere):
def __init__(self, name, vf):
self.name = name
self.vf = vf
self.joints = [name + '/root_joint']
self.handles = dict()
self.handles['low'] = name + '/low'
self.handles['high'] = name + '/high'
vf.loadObjectModel(self.__class__, name)
self.rank = vf.robot.rankInConfiguration[name + '/root_joint']
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
ps.setErrorThreshold(1e-2)
ps.setMaxIterProjection(40)
robot.setJointBounds('romeo/root_joint', [
-1,
1,
-1,
1,
0,
2,
-2.,
2,
-2.,
2,
-2.,
2,
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7,-2,2,-2,2] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5,-2,2,-2,2,-2,2,-2,2])
# 2}}}
# 2}}}
# 1}}}
# Initialization. {{{1
# Set parameters. {{{2
robot.client.basic.problem.resetRoadmap ()
ps.setErrorThreshold (1e-3)
ps.setMaxIterProjection (40)
# 2}}}
# Create lists of joint names - useful to define passive joints. {{{2
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
ps.addPassiveDofs ('pr2', jointNames['pr2'])
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer, PathPlayerGui
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_manipulation'
# Load HRP2 {{{3
robot = Robot('hrp2', 'hrp2')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
robot.setJointBounds("hrp2/base_joint_xyz", [-0.2, 0.8, -0.5, 0.5, 0, 2])
ps.selectPathProjector('Progressive', 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterations(40)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig()
q_init = half_sitting[::]
# Open hands
ilh = robot.rankInConfiguration['hrp2/LARM_JOINT6']
q_init[ilh:ilh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
irh = robot.rankInConfiguration['hrp2/RARM_JOINT6']
q_init[irh:irh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
ibjxyz = robot.rankInConfiguration['hrp2/base_joint_xyz']
q_goal = q_init[::]
q_goal[ibjxyz:ibjxyz + 2] = [0.5, 0]
class Calibration(object):
def __init__(self, client):
self.client = client
self.robot = None
self.robots = list()
self.robot_locks = list()
self.robots_locks = list()
self.robots_gaze = list()
self.robots_identity_constraints = list()
self.q = []
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 setLockJoints(self, robot_id, joints_name_value_tuple):
self.ps.createLockedJoint(
"talos_" + str(robot_id) + "/root_joint",
"talos_" + str(robot_id) + "/root_joint",
[0, 0, 1, 0, 0, 0, 1],
)
root_joint_rank = self.robot.rankInConfiguration[
"talos_" + str(robot_id) + "/root_joint"
]
self.q[root_joint_rank : root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1]
self.robots_locks.append("talos_" + str(robot_id) + "/root_joint")
# self.ps.createLockedJoint("talos_" + str(robot_id) + "/calib_mire_joint_2", "talos_" + str(robot_id) + "/calib_mire_joint_2", [0, 0, 0, 0, 0, 0, 1])
# mire_joint_rank = self.robot.rankInConfiguration["talos_" + str(robot_id) + "/calib_mire_joint_2"]
# self.q[mire_joint_rank:mire_joint_rank + 7] = [0, 0, 0, 0, 0, 0, 1]
# self.robots_locks.append("talos_" + str(robot_id) + "/calib_mire_joint_2")
for name, value in joints_name_value_tuple:
joint_name = "talos_" + str(robot_id) + "/" + name
self.q[self.robot.rankInConfiguration[joint_name]] = value
self.ps.createLockedJoint(joint_name, joint_name, [value])
self.robots_locks.append(joint_name)
def getRankAndConfigSize(self, robot_id):
robot_name = "talos_" + str(robot_id)
rank = self.robot.rankInConfiguration[robot_name + "/root_joint"]
size = sum(
[
self.robot.getJointConfigSize(joint_name)
for joint_name in calib.robot.getJointNames()
if robot_name in joint_name
]
)
return rank, size
def setGaze(self, robot_id, checkerboard_pose):
robot_name = "talos_" + str(robot_id)
position = (
checkerboard_pose.position.x,
checkerboard_pose.position.y,
checkerboard_pose.position.z,
)
orientation = (
checkerboard_pose.orientation.x,
checkerboard_pose.orientation.y,
checkerboard_pose.orientation.z,
checkerboard_pose.orientation.w,
)
self.ps.createPositionConstraint(
robot_name + "gaze",
robot_name + "/calib_rgb_joint",
robot_name + "/calib_mire_joint_2",
position,
(0, 0, 0),
[True, True, True],
)
self.ps.createOrientationConstraint(
robot_name + "gaze_O",
robot_name + "/calib_mire_joint_2",
robot_name + "/calib_rgb_joint",
orientation,
[True, True, True],
)
self.robots_gaze.append(robot_name + "gaze")
self.robots_gaze.append(robot_name + "gaze_O")
def constrainFreeflyers(self):
for robot_id in range(1, len(self.robots)):
robot_name = "talos_" + str(robot_id)
self.client.basic.problem.createIdentityConstraint(
robot_name + "_id_rgb",
["talos_0/calib_rgb_joint"],
[robot_name + "/calib_rgb_joint"],
)
self.client.basic.problem.createIdentityConstraint(
robot_name + "_id_mire",
["talos_0/calib_mire_joint_2"],
[robot_name + "/calib_mire_joint_2"],
)
self.robots_identity_constraints.append(robot_name + "_id_rgb")
self.robots_identity_constraints.append(robot_name + "_id_mire")
def optimize(self, q_init, robots_id):
self.q_proj = self.q
client.basic.problem.resetConstraints()
calib.robot.setCurrentConfig(q_init)
client.basic.problem.addLockedJointConstraints("unused1", calib.robots_locks)
gaze = [
c
for c in calib.robots_gaze
if any(["talos_" + str(robot_id) in c for robot_id in robots_id])
]
num_constraints = gaze + calib.robots_identity_constraints
client.basic.problem.addNumericalConstraints(
"unused2", num_constraints, [0 for _ in num_constraints]
)
# client.basic.problem.addNumericalConstraints("mighty_cam_cost", ["cam_cost"], [ 1 ])
# calib.client.basic.problem.setNumericalConstraintsLastPriorityOptional(True)
projOk, self.q_proj, error = client.basic.problem.applyConstraints(q_init)
if error < 1.0:
optOk, self.q_proj, error = client.basic.problem.optimize(self.q_proj)
if optOk:
print("All was good! " + str(max(error)))
else:
print("Optimisation error: " + str(max(error)))
else:
print("Projection error: " + str(error))
rank_rgb = calib.robot.rankInConfiguration["talos_0/calib_rgb_joint"]
rank_mire = calib.robot.rankInConfiguration["talos_0/calib_mire_joint_2"]
return (
self.q_proj,
self.q_proj[rank_rgb : rank_rgb + 7],
max(error),
self.q_proj[rank_mire : rank_mire + 7],
)
ps.setNumericalConstraints ("test", constraints)
ps.setLockedJointConstraints ("test", lockDofs)
res = [None] * N
q = [None] * N
err = [None] * N
start = time.time()
for i in range(N):
res[i], q[i], err[i] = ps.applyConstraints(qs[i])
duration = time.time() - start
# return res,q,err,duration
return float(res.count(True)) / N,duration / N
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
ps.setErrorThreshold (1e-2)
ps.setMaxIterProjection (40)
robot.setJointBounds ('romeo/root_joint' , [-1,1,-1,1, 0, 2,-2.,2,-2.,2,-2.,2,
-2.,2,])
placard = Placard ('placard', vf)
robot.setJointBounds (placard.name + '/root_joint', [-1,1,-1,1,0,1.5,-2.,2,
-2.,2,-2.,2,-2.,2,])
## Lock both hands
locklhand = list()
for j,v in robot.leftHandOpen.iteritems():
locklhand.append ('romeo/' + j)
if type(v) is float or type(v) is int:
val = [v,]
else:
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'
urdfName = 'ur_benchmark/box'
meshPackageName = 'hpp_practicals'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('ur5-pokeball', 'ur5')
ps = ProblemSolver (robot)
ps.setErrorThreshold (1e-4)
ps.setMaxIterProjection (40)
vf = ViewerFactory (ps)
gripperName = 'ur5/wrist_3_joint'
ballName = 'pokeball/root_joint'
