def createAlignedCOMStabilityConstraint (self, prefix, comName, leftAnkle, rightAnkle, q0, sliding):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition (q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
robot.setCurrentConfig (q0)
x = self._getCOM (robot, comName)
result = []
# COM between feet
result.append (prefix + "com-between-feet")
problem.createComBetweenFeet (result[-1], comName, leftAnkle, rightAnkle,
(0,0,0), (0,0,0), "", x.tolist(), (True,)*4)
if sliding:
mask = ( False, False, True, True, True, False )
else:
mask = ( True, ) * 6
# 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), mask)
# 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), mask)
return result;
def createStaticStabilityConstraint (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)
# 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
def createGraspConstraint(gripperName, handleName):
name = "Relative transformation " + gripperName + "/" + handleName
gjn, gpos = robot.getGripperPositionInJoint(gripperName)
hjn, hpos = robot.getHandlePositionInJoint(handleName)
ps.createTransformationConstraint(
name, gjn, hjn,
(Transform(gpos) * Transform(hpos).inverse()).toTuple(), [True] * 6)
return name
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):
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)
# 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
def setSotFrameFromHpp(self, pinrobot):
# The joint should be available in the robot model used by SOT
n = self.hppjoint
self.sotpose = self.hpppose
self.robotname, self.sotjoint = parseHppName (n)
while self.sotjoint not in pinrobot.names:
self.sotpose = Transform(self.hpp.basic.robot.getJointPositionInParentFrame(n)) * self.sotpose
n = self.hpp.basic.robot.getParentJointName(n)
robotname, self.sotjoint = parseHppName (n)
def selected(self, event):
if event.hasIntersection():
self.pointLabel["local"].text = vec2str(vec(event.point(True)))
self.normalLabel["local"].text = vec2str(vec(event.normal(True)))
self.pointLabel["global"].text = vec2str(vec(event.point(False)))
self.normalLabel["global"].text = vec2str(vec(event.normal(False)))
if len(self.refName.text) == 0:
print self.refName.text
else:
T = Transform(
self.plugin.client.robot.getJointPosition(
str(self.refName.text))).inverse()
try:
self.pointLabel["reference"].text = vec2str(
T.transform(vec(event.point(False))))
self.normalLabel["reference"].text = vec2str(
T.quaternion.transform(vec(event.normal(False))))
except ValueError as e:
print e
print event.point(False)
print vec(event.point(False))
event.done()
def addAlignmentConstrainttoEdge(ps, handle, graph):
#recover id of handle
handleId = all_handles.index(handle)
J1, gripperPose = ps.robot.getGripperPositionInJoint(tool_gripper)
J2, handlePose = ps.robot.getHandlePositionInJoint(handle)
T1 = Transform(gripperPose)
T2 = Transform(handlePose)
constraintName = handle + '/alignment'
ps.client.basic.problem.createTransformationConstraint2\
(constraintName, J1, J2, T1.toTuple(), T2.toTuple(),
[False, True, True, False, False, False])
# Set constraint
edgeName = tool_gripper + ' > ' + handle + ' | 0-0_12'
graph.addConstraints(edge = edgeName, constraints = \
Constraints(numConstraints=[constraintName]))
edgeName = tool_gripper + ' < ' + handle + ' | 0-0:1-{}_21'.format(handleId)
graph.addConstraints(edge = edgeName, constraints = \
Constraints(numConstraints=[constraintName]))
def createAlignedCOMStabilityConstraint(self, prefix, comName, leftAnkle,
rightAnkle, q0, sliding):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition(q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
robot.setCurrentConfig(q0)
x = self._getCOM(robot, comName)
result = []
# COM between feet
result.append(prefix + "com-between-feet")
problem.createComBetweenFeet(
result[-1],
comName,
leftAnkle,
rightAnkle,
(0, 0, 0),
(0, 0, 0),
"",
x.tolist(),
(True, ) * 4,
)
if sliding:
mask = (False, False, True, True, True, False)
else:
mask = (True, ) * 6
# 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), mask)
# 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), mask)
return result
ps.createLockedJoint("gripper_l_lock_f1j", "talos/gripper_left_fingertip_1_joint", [0])
ps.createLockedJoint("gripper_l_lock_f2j", "talos/gripper_left_fingertip_2_joint", [0])
ps.createLockedJoint("gripper_l_lock_isj", "talos/gripper_left_inner_single_joint", [0])
ps.createLockedJoint("gripper_l_lock_f3j", "talos/gripper_left_fingertip_3_joint", [0])
ps.createLockedJoint("gripper_l_lock_j", "talos/gripper_left_joint", [0])
ps.createLockedJoint("gripper_l_lock_msj", "talos/gripper_left_motor_single_joint", [0])
# lock drawer closed
ps.createLockedJoint("drawer_lock", "desk/upper_case_bottom_upper_drawer_bottom_joint", [0])
# static stability constraint
ps.addPartialCom("Talos_CoM", ["talos/root_joint"])
ps.createStaticStabilityConstraints("talos_static_stability", q, "Talos_CoM", ProblemSolver.FIXED_ON_THE_GROUND)
tpc = ps.getPartialCom("Talos_CoM")
rla = robot.getJointPosition(robot.leftAnkle)
com_wf = np.array(tpc)
tf_la = Transform(rla)
com_la = tf_la.inverse().transform(com_wf)
ps.createRelativeComConstraint("talos_com_constraint_to_la", "Talos_CoM", robot.leftAnkle, com_la.tolist(), (True, True, False))
ps.setMaxIterProjection(40) # Set the maximum number of iterations of the projection algorithm. Must be called before the graph creation.
# constraint graph
rules = [
Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "^$"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_top"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_front"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_front", "^$"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_drawer", "^$"], True),
Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/upper_drawer_spherical_handle"], True)
]
## 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
def getObjectPosition(self, objectName):
return Transform(self.hppcorba.robot.getObjectPosition(objectName))
if len(paths) == 0: return None
p = paths[0].asVector()
for q in paths[1:]:
p.appendPath(q)
return p
# 2}}}
# {{{2 Problem resolution
# {{{3 Finalize FOV filter
res, q, err = graph.applyNodeConstraints(free, q0)
assert res
robot.setCurrentConfig(q)
oMh, oMd = robot.hppcorba.robot.getJointsPosition(q, ["tiago/hand_tool_link", "driller/base_link"])
tiago_fov.appendUrdfModel(Driller.urdfFilename, "hand_tool_link",
(Transform(oMh).inverse() * Transform(oMd)).toTuple(),
prefix="driller/")
# 3}}}
# {{{3 Create InStatePlanner
armPlanner = InStatePlanner ()
armPlanner.setEdge(loop_free)
#armPlanner.optimizerTypes = [ "SplineGradientBased_bezier3", ]
armPlanner.optimizerTypes = [ ]
armPlanner.cproblem.setParameter("SimpleTimeParameterization/safety", Any(TC_float, 0.25))
armPlanner.cproblem.setParameter("SimpleTimeParameterization/order", Any(TC_long, 2))
armPlanner.cproblem.setParameter("SimpleTimeParameterization/maxAcceleration", Any(TC_float, 1.0))
armPlanner.maxIterPathPlanning = 600
armPlanner.timeOutPathPlanning = 10.
# Set collision margin between mobile base and the rest because the collision model is not correct.
bodies = ("tiago/torso_fixed_link_0", "tiago/base_link_0")
p.appendPath(q)
return p
# 2}}}
# {{{2 Problem resolution
# {{{3 Finalize FOV filter
res, q, err = graph.applyNodeConstraints(free, q0)
assert res
robot.setCurrentConfig(q)
oMh, oMd = robot.hppcorba.robot.getJointsPosition(
q, ["tiago/hand_tool_link", "driller/base_link"])
tiago_fov.appendUrdfModel(Driller.urdfFilename,
"hand_tool_link", (Transform(oMh).inverse() *
Transform(oMd)).toTuple(),
prefix="driller/")
# 3}}}
# {{{3 Create InStatePlanner
armPlanner = InStatePlanner()
armPlanner.setEdge(loop_free)
# Set collision margin between mobile base and the rest because the collision model is not correct.
bodies = ("tiago/torso_fixed_link_0", "tiago/base_link_0")
cfgVal = wd(armPlanner.cproblem.getConfigValidations())
pathVal = wd(armPlanner.cproblem.getPathValidation())
for _, la, lb, _, _ in zip(*robot.distancesToCollision()):
if la in bodies or lb in bodies:
cfgVal.setSecurityMarginBetweenBodies(la, lb, 0.07)
pathVal.setSecurityMarginBetweenBodies(la, lb, 0.07)
def setHppGripper (self, name):
self.name = name
# Get parent joint and position from HPP
self.hppjoint, self.hpppose = self.hpp.manipulation.robot.getGripperPositionInJoint(name)
self.hpppose = Transform (self.hpppose)
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)
## 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
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"):
def getObjectPosition(self, objectName):
return Transform(self.client.basic.robot.getObjectPosition(objectName))
## 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
