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:
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,
])
robot.client.basic.problem.setMaxIterations (40)
# 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'])
-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))
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]
# 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'])
