]
# "rightfoot/rxryrz"]]
ps.createRelativeComConstraint(rightfoot[0][0], "", robot.rightAnkle,
(0, 0, 0.7028490491159864), (True, True, True))
ps.createPositionConstraint(rightfoot[0][1], robot.rightAnkle, "", (0, 0, 0),
rightpos[0:3], (False, False, True))
ps.createPositionConstraint(rightfoot[1][0], robot.rightAnkle, "", (0, 0, 0),
rightpos[0:3], (True, True, False))
ps.client.basic.problem.setConstantRightHandSide(rightfoot[1][0], False)
ps.createOrientationConstraint(rightfoot[0][2], robot.rightAnkle, "",
(1, 0, 0, 0), (True, True, True))
# ps.createOrientationConstraint (rightfoot[1][2], robot.rightAnkle, "", (1,0,0,0), (False, False, True))
# ps.client.basic.problem.setConstantRightHandSide (rightfoot[1][2], True)
ps.createStaticStabilityConstraints("both",
q_init,
type=ProblemSolver.ALIGNED_COM)
bothfeet = [[
"both/com-between-feet",
], []]
bothfeet[0].extend(leftfoot[0][1:])
bothfeet[0].extend(rightfoot[0][1:])
bothfeet[1].extend(leftfoot[1][:])
bothfeet[1].extend(rightfoot[1][:])
# Waist should stay in a good general shape.
# ps.createOrientationConstraint ("all/ori_waist", robot.waist, "", (1,0,0,0), (True, True, True))
ps.createLockedJoint("all/ori_waist", robot.waist, (1, 0, 0, 0))
# lock hands {{{4
lockhand = [
'larm_6', 'lhand_0', 'lhand_1', 'lhand_2', 'lhand_3', 'lhand_4', 'rarm_6',
v,
]
else:
val = v
ps.createLockedJoint('romeo/' + j, robot.displayName + '/' + j, val)
lockHands = lockrhand + locklhand
## Create static stability constraint
robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom('romeo', ['romeo/root_joint'])
q = robot.getInitialConfig()
r = placard.rank
q[r:r + 3] = [.4, 0, 1.2]
ps.createStaticStabilityConstraints('balance-romeo',
q,
'romeo',
type=ProblemSolver.FIXED_ON_THE_GROUND)
commonConstraints = Constraints(numConstraints=ps.balanceConstraints(),
lockedJoints=lockHands)
# build graph
rules = [
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["placard/low", ""], True),
Rule([
"romeo/l_hand",
"romeo/r_hand",
], ["", "placard/high"], True),
Rule([
ps.createLockedJoint("gripper_r_lock_j", "talos/gripper_right_joint", [0])
ps.createLockedJoint("gripper_r_lock_msj", "talos/gripper_right_motor_single_joint", [0])
# lock gripper left open
ps.createLockedJoint("gripper_l_lock_idj", "talos/gripper_left_inner_double_joint", [0])
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),
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))
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 = []
# [q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])
# lockAll = lockhands + lockHeadAndTorso + lockKitchen
lockAll = lockhands + lockKitchen
# 4}}}
# Create constraints corresponding to each object {{{4
lockFridge = ["fridge_lock", ]
ps.createLockedJoint ("fridge_lock", Kitchen.joint, [0,])
lockCup = ps.lockFreeFlyerJoint (Cup.joint, 'cup_lock')
robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom ('romeo', ['romeo/base_joint_xyz'])
ps.createStaticStabilityConstraints ("balance-romeo", q_init, 'romeo')
# 4}}}
# 3}}}
# Create the graph. {{{3
cg.createNode (['both', 'cup', 'fridge', 'free'])
# Cup and fridge {{{4
cg.setConstraints (node = 'both', grasps = [ 'l_cup_grasp', 'r_fridge_grasp', ])
cg.createWaypointEdge ('fridge', 'both', 'l_cup_grasp_both', 1, 10)
cg.setConstraints (edge='l_cup_grasp_both_e1', lockDof = lockCup)
cg.setConstraints (node='l_cup_grasp_both_n0', grasps = ['r_fridge_grasp'], pregrasps = ['l_cup_pregrasp',])
lockrhand.append ('romeo/' + j)
if type(v) is float or type(v) is int:
val = [v,]
else:
val = v;
ps.createLockedJoint ('romeo/' + j, robot.displayName + '/' + j, val)
lockHands = lockrhand + locklhand
## Create static stability constraint
robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom ('romeo', ['romeo/root_joint'])
q = robot.getInitialConfig ()
r = placard.rank
q [r:r+3] = [.4, 0, 1.2]
ps.createStaticStabilityConstraints ('balance-romeo', q, 'romeo',
type = ProblemSolver.FIXED_ON_THE_GROUND)
commonConstraints = Constraints (numConstraints = ps.balanceConstraints (),
lockedJoints = lockHands)
# build graph
rules = [Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", ""], True),
Rule (["romeo/l_hand","romeo/r_hand",], ["", "placard/high"], True),
Rule (["romeo/l_hand","romeo/r_hand",], ["placard/low", "placard/high"], True),
]
grippers = ['romeo/r_hand', 'romeo/l_hand']
handlesPerObjects = [placard.handles.values ()]
cg = ConstraintGraph.buildGenericGraph (robot, "graph",
grippers,
[placard.name,],
p.createLockedDofConstraint(lockbottompart[-1], n, 0, 1, 0)
p.isLockedDofParametric(lockbottompart[-1], True)
lockbottompart.append("lock" + n + '_y')
p.createLockedDofConstraint(lockbottompart[-1], n, 0, 2, 1)
p.isLockedDofParametric(lockbottompart[-1], True)
lockbottompart.append("lock" + n + '_z')
p.createLockedDofConstraint(lockbottompart[-1], n, 0, 3, 2)
p.isLockedDofParametric(lockbottompart[-1], True)
else:
lockbottompart.append("lock" + n)
p.createLockedDofConstraint(lockbottompart[-1], n, 0, 0, 0)
p.isLockedDofParametric(lockbottompart[-1], True)
p.createStaticStabilityConstraints("balance", q1)
#p.createComplementStaticStabilityConstraints ("c-balance", q1)
##cBalance = ["c-balance/c-orientation-left-foot","c-balance/c-position-left-foot"]
#cBalance = ["c-balance/c-position-left-foot"]
#p.client.basic.problem.isParametric ("c-balance/c-orientation-left-foot", True)
#p.client.basic.problem.isParametric ("c-balance/c-position-left-foot", True)
graph = robot.client.manipulation.graph
id = dict()
id["graph"] = graph.createGraph('hrp2-screwgun')
id["subgraph"] = graph.createSubGraph('lefthand')
id["screwgun"] = graph.createNode(id["subgraph"], 'screwgun')
id["prescrewgun"] = graph.createNode(id["subgraph"], 'prescrewgun')
id["free"] = graph.createNode(id["subgraph"], 'free')
id["unpregrasp"] = graph.createEdge(id["prescrewgun"], id["free"],
"unpregrasp", 1, False)
rightfoot = [ [
# "rightfoot/comz" ,
"rightfoot/com", "rightfoot/z" , "rightfoot/rxry"],
["rightfoot/xy",
]]
# "rightfoot/rxryrz"]]
ps.createRelativeComConstraint (rightfoot[0][0], "", robot.rightAnkle, (0,0,0.7028490491159864), (True,True,True))
ps.createPositionConstraint (rightfoot[0][1], robot.rightAnkle, "", (0,0,0), rightpos[0:3], (False, False, True))
ps.createPositionConstraint (rightfoot[1][0], robot.rightAnkle, "", (0,0,0), rightpos[0:3], (True, True, False))
ps.client.basic.problem.setConstantRightHandSide (rightfoot[1][0], False)
ps.createOrientationConstraint (rightfoot[0][2], robot.rightAnkle, "", (1,0,0,0), (True, True, True))
# ps.createOrientationConstraint (rightfoot[1][2], robot.rightAnkle, "", (1,0,0,0), (False, False, True))
# ps.client.basic.problem.setConstantRightHandSide (rightfoot[1][2], True)
ps.createStaticStabilityConstraints ("both", q_init, type = ProblemSolver.ALIGNED_COM)
bothfeet = [ ["both/com-between-feet", ],
[] ]
bothfeet[0].extend (leftfoot[0][1:])
bothfeet[0].extend (rightfoot[0][1:])
bothfeet[1].extend (leftfoot[1][:])
bothfeet[1].extend (rightfoot[1][:])
# Waist should stay in a good general shape.
# ps.createOrientationConstraint ("all/ori_waist", robot.waist, "", (1,0,0,0), (True, True, True))
ps.createLockedJoint ("all/ori_waist", robot.waist, (1,0,0,0))
# lock hands {{{4
lockhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4',
'rarm_6','rhand_0','rhand_1','rhand_2','rhand_3','rhand_4']
ps.createLockedJoint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
ps.createLockedJoint ('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
# 4}}}
# Create constraints corresponding to each object {{{4
lockFridge = [
"fridge_lock",
]
ps.createLockedJoint("fridge_lock", Kitchen.joint, [
0,
])
lockCup = ps.lockFreeFlyerJoint(Cup.joint, 'cup_lock')
robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom('romeo', ['romeo/base_joint_xyz'])
ps.createStaticStabilityConstraints("balance-romeo", q_init, 'romeo')
# 4}}}
# 3}}}
# Create the graph. {{{3
cg.createNode(['both', 'cup', 'fridge', 'free'])
# Cup and fridge {{{4
cg.setConstraints(node='both', grasps=[
'l_cup_grasp',
'r_fridge_grasp',
])
cg.createWaypointEdge('fridge', 'both', 'l_cup_grasp_both', 1, 10)
locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
ps.createLockedJoint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
ps.createLockedJoint \
('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
ps.createLockedJoint \
('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJoint \
('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJoint \
('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJoint \
('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
ps.addPartialCom ('hrp2', ['hrp2/base_joint_xyz'])
ps.createStaticStabilityConstraints ("balance-hrp2", q_init, 'hrp2')
# 3}}}
# Create the graph of constraints {{{3
graph.createNode (["screwgun", "free"])
graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1)
graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5)
graph.createEdge ('free', 'free', 'move_free', 5)
graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10)
graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5)
graph.setConstraints (node='screwgun', grasp='l_grasp')
lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock')
locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
ps.createLockedJointConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
ps.createLockedJointConstraint \
('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
ps.createLockedJointConstraint \
('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJointConstraint \
('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJointConstraint \
('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJointConstraint \
('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
ps.createStaticStabilityConstraints ("balance", q_init)
# 3}}}
# Create the graph of constraints {{{3
graph.createNode (["screwgun", "free"])
graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1)
graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5)
graph.createEdge ('free', 'free', 'move_free', 5)
graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10)
graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5)
graph.setConstraints (node='screwgun', grasp='l_grasp')
ps.createLockedJoint('larm_6', 'hrp2/LARM_JOINT6', [
q_init[ilh],
])
ps.createLockedJoint \
('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
ps.createLockedJoint \
('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJoint \
('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJoint \
('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJoint \
('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
ps.addPartialCom('hrp2', ['hrp2/base_joint_xyz'])
ps.createStaticStabilityConstraints("balance-hrp2", q_init, 'hrp2')
# 3}}}
# Create the graph of constraints {{{3
graph.createNode(["screwgun", "free"])
graph.createWaypointEdge('screwgun', 'free', 'ungrasp', nb=1, weight=1)
graph.createWaypointEdge('free', 'screwgun', 'grasp', nb=1, weight=5)
graph.createEdge('free', 'free', 'move_free', 5)
graph.createEdge('screwgun', 'screwgun', 'keep_grasp', 10)
graph.createLevelSetEdge('screwgun', 'screwgun', 'keep_grasp_ls', 5)
graph.setConstraints(node='screwgun', grasp='l_grasp')
p.createLockedDofConstraint (lockbottompart[-1], n, 0, 1 ,0)
p.isLockedDofParametric (lockbottompart[-1], True)
lockbottompart.append ("lock"+n+'_y')
p.createLockedDofConstraint (lockbottompart[-1], n, 0, 2 ,1)
p.isLockedDofParametric (lockbottompart[-1], True)
lockbottompart.append ("lock"+n+'_z')
p.createLockedDofConstraint (lockbottompart[-1], n, 0, 3 ,2)
p.isLockedDofParametric (lockbottompart[-1], True)
else:
lockbottompart.append ("lock"+n)
p.createLockedDofConstraint (lockbottompart[-1], n, 0, 0, 0)
p.isLockedDofParametric (lockbottompart[-1], True)
p.createStaticStabilityConstraints ("balance", q1)
#p.createComplementStaticStabilityConstraints ("c-balance", q1)
##cBalance = ["c-balance/c-orientation-left-foot","c-balance/c-position-left-foot"]
#cBalance = ["c-balance/c-position-left-foot"]
#p.client.basic.problem.isParametric ("c-balance/c-orientation-left-foot", True)
#p.client.basic.problem.isParametric ("c-balance/c-position-left-foot", True)
graph = robot.client.manipulation.graph
id = dict()
id["graph" ] = graph.createGraph ('hrp2-screwgun')
id["subgraph"] = graph.createSubGraph ('lefthand')
id["screwgun"] = graph.createNode (id["subgraph"], 'screwgun')
id["prescrewgun"] = graph.createNode (id["subgraph"], 'prescrewgun')
id["free" ] = graph.createNode (id["subgraph"], 'free')
id["unpregrasp"] = graph.createEdge (id["prescrewgun"], id["free"], "unpregrasp", 1, False)
id[ "pregrasp"] = graph.createEdge (id["free"], id["prescrewgun"], "pregrasp", 10, True)
