def createSlidingStabilityConstraint (self, prefix, comName, leftAnkle, rightAnkle, q0):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition (q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
self.setCurrentConfig (q0)
x = self._getCOM (comName)
result = []
# COM wrt left ankle frame
xloc = Ml.inverse().transform(x)
result.append (prefix + "relative-com")
problem.createRelativeComConstraint (result[-1], comName, leftAnkle, xloc.tolist(), (True,)*3)
# Relative pose of the feet
result.append (prefix + "relative-pose")
problem.createTransformationConstraint2 (result[-1],
leftAnkle, rightAnkle, (0,0,0,0,0,0,1), (Mr.inverse()*Ml).toTuple(), (True,)*6)
# Pose of the left foot
result.append (prefix + "pose-left-foot")
problem.createTransformationConstraint2 (result[-1],
"", leftAnkle, Ml.toTuple(), (0,0,0,0,0,0,1), (False,False,True,True,True,False))
# Complement left foot
result.append (prefix + "pose-left-foot-complement")
problem.createTransformationConstraint2 (result[-1],
"", leftAnkle, Ml.toTuple(), (0,0,0,0,0,0,1), (True,True,False,False,False,True))
problem.setConstantRightHandSide (result[-1], False)
return result
def createSlidingStabilityConstraint(self, prefix, comName, leftAnkle,
rightAnkle, q0):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition(q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
self.setCurrentConfig(q0)
x = self._getCOM(comName)
result = []
# COM wrt left ankle frame
xloc = Ml.inverse().transform(x)
result.append(prefix + "relative-com")
problem.createRelativeComConstraint(result[-1], comName, leftAnkle,
xloc.tolist(), (True, ) * 3)
# Relative pose of the feet
result.append(prefix + "relative-pose")
problem.createTransformationConstraint2(
result[-1],
leftAnkle,
rightAnkle,
(0, 0, 0, 0, 0, 0, 1),
(Mr.inverse() * Ml).toTuple(),
(True, ) * 6,
)
# Pose of the left foot
result.append(prefix + "pose-left-foot")
problem.createTransformationConstraint2(
result[-1],
"",
leftAnkle,
Ml.toTuple(),
(0, 0, 0, 0, 0, 0, 1),
(False, False, True, True, True, False),
)
# Complement left foot
result.append(prefix + "pose-left-foot-complement")
problem.createTransformationConstraint2(
result[-1],
"",
leftAnkle,
Ml.toTuple(),
(0, 0, 0, 0, 0, 0, 1),
(True, True, False, False, False, True),
)
problem.setConstantRightHandSide(result[-1], False)
return result
def validateGazeConstraint(ps, q, whichArm):
robot = ps.robot
robot.setCurrentConfig(q)
Mcamera = Transform(robot.getLinkPosition("talos/rgbd_optical_frame"))
Mtarget = Transform(
robot.getLinkPosition("talos/arm_" + whichArm + "_7_link"))
z = (Mcamera.inverse() * Mtarget).translation[2]
if z < .1: return False
return True
def createStaticStabilityConstraint(self,
prefix,
comName,
leftAnkle,
rightAnkle,
q0,
maskCom=(True, ) * 3):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition(q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
self.setCurrentConfig(q0)
x = self._getCOM(comName)
result = []
# COM wrt left ankle frame
xloc = Ml.inverse().transform(x)
result.append(prefix + "relative-com")
problem.createRelativeComConstraint(result[-1], comName, leftAnkle,
xloc.tolist(), maskCom)
# Pose of the left foot
result.append(prefix + "pose-left-foot")
problem.createTransformationConstraint2(
result[-1],
"",
leftAnkle,
Ml.toTuple(),
(0, 0, 0, 0, 0, 0, 1),
(True, True, True, True, True, True),
)
# Pose of the right foot
result.append(prefix + "pose-right-foot")
problem.createTransformationConstraint2(
result[-1],
"",
rightAnkle,
Mr.toTuple(),
(0, 0, 0, 0, 0, 0, 1),
(True, True, True, True, True, True),
)
return result
_) 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)
i += 1
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])
right_gripper_lock.append(n)
## Project initial configuration on state 'placement'
res, q_init, error = graph.applyNodeConstraints ('placement', q1)
q2 = q1 [::]
q2 [7] = .2
## Project goal configuration on state 'placement'
res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# v = vf.createViewer ()
# pp = PathPlayer (v)
# v (q1)
## Build relative position of the ball with respect to the gripper
for i in range (100):
q = robot.shootRandomConfig ()
res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q)
if res and robot.isConfigValid (q3): break;
if res:
robot.setCurrentConfig (q3)
gripperPose = Transform (robot.getJointPosition (gripperName))
ballPose = Transform (robot.getJointPosition (ballName))
gripperGraspsBall = gripperPose.inverse () * ballPose
gripperAboveBall = Transform (gripperGraspsBall)
gripperAboveBall.translation [2] += .1
## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
(graph.nodes ['placement'], q1)
q2 = q1 [::]
q2 [7] = .2
## Project goal configuration on state 'placement'
res, q_goal, error = ps.client.manipulation.problem.applyConstraints \
(graph.nodes ['placement'], q2)
## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)
pp = PathPlayer (ps.client.basic, r)
## Build relative position of the ball with respect to the gripper
for i in range (100):
q = robot.shootRandomConfig ()
res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q)
if res and robot.isConfigValid (q3): break;
if res:
robot.setCurrentConfig (q3)
gripperPose = Transform (robot.getLinkPosition (gripperName))
ballPose = Transform (robot.getLinkPosition (ballName))
gripperAboveBall = gripperPose.inverse () * ballPose
gripperAboveBall.translation [2] += .1
# 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"):
ps.createLockedJoint(n, n, half_sitting[r:r + s])
## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
(graph.nodes ['placement'], q1)
q2 = q1[::]
q2[7] = .2
## Project goal configuration on state 'placement'
res, q_goal, error = ps.client.manipulation.problem.applyConstraints \
(graph.nodes ['placement'], q2)
## Define manipulation planning problem
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.selectPathValidation("Dichotomy", 0)
pp = PathPlayer(ps.client.basic, r)
## Build relative position of the ball with respect to the gripper
for i in range(100):
q = robot.shootRandomConfig()
res, q3, err = graph.generateTargetConfig('grasp-ball', q_init, q)
if res and robot.isConfigValid(q3): break
if res:
robot.setCurrentConfig(q3)
gripperPose = Transform(robot.getLinkPosition(gripperName))
ballPose = Transform(robot.getLinkPosition(ballName))
gripperAboveBall = gripperPose.inverse() * ballPose
gripperAboveBall.translation[2] += .1