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
Robot.srdfSuffix= ''
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'cup'
urdfSuffix = ""
srdfSuffix = ""
handles = [ "box/top", "box/bottom" ]
robot = Robot ('dev', 'talos', rootJointType = "freeflyer")
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)
Object = Box
vf.loadObjectModel (Object, 'box')
robot.setJointBounds ("box/root_joint", [-1, 1, -1, 1, 0, 2])
qq = [-0.7671778026566639, 0.0073267002287253635, 1.0035168727631776, -0.003341673452654457, -0.021566597515109524, 0.0002183620894239602, 0.9997618053357284, -0.00020053128587844821, -0.0021365695604276275, -0.4415951773681094, 0.9659230706255528, -0.48119003672520416, 0.007109157982067145, -0.00020095991543181877, -0.002126639473414498, -0.4382848597339842, 0.9589221865248464, -0.4774994711722908, 0.007099218648561522, 0.0, 0.025235347910697536, -0.06985947194357875, 0.0, -0.12446173084176845, -1.5808415926365578, 0.014333078135875619, -0.0806417043955706, -0.37124401660668394, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25955282922987977, -0.1618313202464181, 0.002447883426630002, -0.5149037074691503, -0.00010703274362664899, 0.0008742582163227642, 0.10168585913285667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.785398163397, 0.32250041955468695, -0.2569883469655496, 0.18577095561452217, 1.164388709412583, 0.0694401264431558, 0.5475575114527793, -0.11842286843715424, 0.8254301089264399]
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('ur5-box', 'ur5', rootJointType = 'anchor')
ps = ProblemSolver (robot)
fk = ViewerFactory (ps)
fk.loadObjectModel (Box, 'box')
fk.buildCompositeRobot (['ur5', 'box'])
robot.setJointBounds ("box/base_joint_xyz", [-2,+2,-2,+2,-2,2])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['ur5/shoulder_lift_joint']
q_init [rank] = 0
rank = robot.rankInConfiguration ['ur5/elbow_joint']
q_init [rank] = - PI / 2
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_init [rank:rank+3] = [1,1,1]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_init [rank:rank+4] = [1,0,0,0]
q_goal = q_init [:]
rank = robot.rankInConfiguration ['ur5/shoulder_lift_joint']
class Box(object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot('ur5-box', 'ur5', rootJointType='anchor')
ps = ProblemSolver(robot)
fk = ViewerFactory(ps)
fk.loadObjectModel(Box, 'box')
fk.buildCompositeRobot(['ur5', 'box'])
robot.setJointBounds("box/base_joint_xyz", [-2, +2, -2, +2, -2, 2])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig()
rank = robot.rankInConfiguration['ur5/shoulder_lift_joint']
q_init[rank] = 0
rank = robot.rankInConfiguration['ur5/elbow_joint']
q_init[rank] = -PI / 2
rank = robot.rankInConfiguration['box/base_joint_xyz']
q_init[rank:rank + 3] = [1, 1, 1]
rank = robot.rankInConfiguration['box/base_joint_SO3']
q_init[rank:rank + 4] = [1, 0, 0, 0]
q_goal = q_init[:]
rank = robot.rankInConfiguration['ur5/shoulder_lift_joint']
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],
)
