t.translation.y = transform.translation[1]
t.translation.z = transform.translation[2]
publisher.publish(t)
client = CorbaClient()
bag = rosbag.Bag("/usr/localDev/rosbag-calib/pyrene-calib.bag")
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
robot = Robot("talos", "talos", rootJointType="freeflyer", client=client)
q = robot.getCurrentConfig()
pub_cMo = rospy.Publisher("/camera_object", TransformROS, queue_size=100)
pub_fMe = rospy.Publisher("/world_effector", TransformROS, queue_size=100)
rospy.init_node("pose_sender", anonymous=True)
i = 0
for (_, joint_states, _), (_, checkerboard_pose,
_) in zip(bag.read_messages(topics=["joints"]),
bag.read_messages(topics=["chessboard"])):
root_joint_rank = robot.rankInConfiguration["talos/root_joint"]
q[root_joint_rank:root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1]
joints_name_value_tuple = zip(joint_states.name, joint_states.position)
for name, value in joints_name_value_tuple:
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']
q_goal [rank] = - PI
rank = robot.rankInConfiguration ['ur5/elbow_joint']
q_goal [rank] = PI / 2
# 3}}}
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]
half_sitting = [
-0.74,0,1.0192720229567027,0,0,0,1, # root_joint
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_right
0, 0.006761, # torso
0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, # arm_left
0, 0, 0, 0, 0, 0, 0, # gripper_left
-0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, # arm_right
0, 0, 0, 0, 0, 0, 0, # gripper_right
0, 0, # head
0,0,0,0,0,0,1, # box
]
q_init = robot.getCurrentConfig()
ps.addPartialCom ("talos", ["talos/root_joint"])
ps.addPartialCom ("talos_box", ["talos/root_joint", "box/root_joint"])
ps.createStaticStabilityConstraints ("balance", half_sitting, "talos", ProblemSolver.FIXED_ON_THE_GROUND)
foot_placement = [ "balance/pose-left-foot", "balance/pose-right-foot" ]
foot_placement_complement = [ ]
robot.setCurrentConfig(half_sitting)
com_wf = np.array(ps.getPartialCom("talos"))
tf_la = Transform (robot.getJointPosition(robot.leftAnkle))
com_la = tf_la.inverse().transform(com_wf)
ps.createRelativeComConstraint ("com_talos_box", "talos_box", robot.leftAnkle, com_la.tolist(), (True, True, True))
ps.createRelativeComConstraint ("com_talos" , "talos" , robot.leftAnkle, com_la.tolist(), (True, True, True))
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],
)
