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: joint_name = "talos/" + name q[robot.rankInConfiguration[joint_name]] = value robot.setCurrentConfig(q) gripper = Transform( robot.getJointPosition("talos/gripper_left_base_link_joint")) camera = Transform(robot.getJointPosition("talos/rgbd_rgb_optical_joint")) fMe = gripper.inverse() * camera cMo = Transform([ checkerboard_pose.pose.position.x, checkerboard_pose.pose.position.y, checkerboard_pose.pose.position.z, checkerboard_pose.pose.orientation.x, checkerboard_pose.pose.orientation.y, checkerboard_pose.pose.orientation.z, checkerboard_pose.pose.orientation.w, ]) publishTransform(pub_cMo, cMo) publishTransform(pub_fMe, fMe)
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)) ps.createPositionConstraint ("gaze", "talos/rgbd_optical_joint", "box/root_joint", (0,0,0), (0,0,0), (True, True, False)) left_gripper_lock = [] right_gripper_lock = [] other_lock = ["talos/torso_1_joint"] for n in robot.jointNames: s = robot.getJointConfigSize(n) r = robot.rankInConfiguration[n] if n.startswith ("talos/gripper_right"): ps.createLockedJoint(n, n, half_sitting[r:r+s])
return coord[0:2, 0] # END INIT CHESSBOARD AND CAMERA PARAM AND FUNCTIONS ps.addPartialCom("talos", ["talos/root_joint"]) ps.addPartialCom("talos_mire", ["talos/root_joint", "mire/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_mire", "talos_mire", robot.leftAnkle, com_la.tolist(), (True, True, True)) ps.createRelativeComConstraint("com_talos", "talos", robot.leftAnkle, com_la.tolist(), (True, True, True)) left_gripper_lock = [] right_gripper_lock = [] head_lock = [] other_lock = ["talos/torso_1_joint"] for n in robot.jointNames: s = robot.getJointConfigSize(n) r = robot.rankInConfiguration[n] if n.startswith("talos/gripper_right"):
# Create constraints. {{{3 lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock', values=(1, 1, 1, 1,0,0,0)) ps.createLockedJoint ('shoulder_pan', 'ur5/shoulder_pan_joint', [0]) ps.createLockedJoint ('wrist_1', 'ur5/wrist_1_joint', [0]) ps.createLockedJoint ('wrist_2', 'ur5/wrist_2_joint', [0]) ps.createLockedJoint ('wrist_3', 'ur5/wrist_3_joint', [0]) lockur5 = ['shoulder_pan','wrist_1', 'wrist_2', 'wrist_3']; lockjoints = list () lockjoints.extend (lockbox) lockjoints.extend (lockur5) robot.setCurrentConfig (q_init) tf = robot.getJointPosition ('ur5/wrist_1_joint') #robot.client.basic.problem.createPositionConstraint \ #('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True)) robot.client.basic.problem.createPositionConstraint \ ('pos', '', 'ur5/wrist_1_joint', (0, 0, tf[2]), (0,0,0), (False, False, True)) # 3}}} # Create the graph. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') cg.createNode (['free']) cg.setConstraints (node = 'free', numConstraints = ['pos'])
lockbox = ps.lockFreeFlyerJoint('box/base_joint', 'box_lock', values=(1, 1, 1, 1, 0, 0, 0)) ps.createLockedJoint('shoulder_pan', 'ur5/shoulder_pan_joint', [0]) ps.createLockedJoint('wrist_1', 'ur5/wrist_1_joint', [0]) ps.createLockedJoint('wrist_2', 'ur5/wrist_2_joint', [0]) ps.createLockedJoint('wrist_3', 'ur5/wrist_3_joint', [0]) lockur5 = ['shoulder_pan', 'wrist_1', 'wrist_2', 'wrist_3'] lockjoints = list() lockjoints.extend(lockbox) lockjoints.extend(lockur5) robot.setCurrentConfig(q_init) tf = robot.getJointPosition('ur5/wrist_1_joint') #robot.client.basic.problem.createPositionConstraint \ #('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True)) robot.client.basic.problem.createPositionConstraint \ ('pos', '', 'ur5/wrist_1_joint', (0, 0, tf[2]), (0,0,0), (False, False, True)) # 3}}} # Create the graph. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph(robot, 'graph') cg.createNode(['free']) cg.setConstraints(node='free', numConstraints=['pos'])