leftpos = robot.getJointPosition(robot.leftAnkle)
rightpos = robot.getJointPosition(robot.rightAnkle)

leftfoot = [
    [
        # "leftfoot/comz" ,
        "leftfoot/com",
        "leftfoot/z",
        "leftfoot/rxry"
    ],
    [
        "leftfoot/xy",
    ]
]
# "leftfoot/rxryrz"]]
ps.createRelativeComConstraint(leftfoot[0][0], "", robot.leftAnkle,
                               (0, 0, 0.7028490491159864), (True, True, True))
ps.createPositionConstraint(leftfoot[0][1], robot.leftAnkle, "", (0, 0, 0),
                            leftpos[0:3], (False, False, True))
ps.createPositionConstraint(leftfoot[1][0], robot.leftAnkle, "", (0, 0, 0),
                            leftpos[0:3], (True, True, False))
ps.client.basic.problem.setConstantRightHandSide(leftfoot[1][0], False)
ps.createOrientationConstraint(leftfoot[0][2], robot.leftAnkle, "",
                               (1, 0, 0, 0), (True, True, True))
# ps.createOrientationConstraint (leftfoot[1][2], robot.leftAnkle, "", (1,0,0,0), (False, False, True))
# ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][2], True)

rightfoot = [
    [
        # "rightfoot/comz" ,
        "rightfoot/com",
        "rightfoot/z",
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)
		]

graph = ConstraintGraph(robot, "graph")
factory = ConstraintGraphFactory(graph)
Exemple #3
0
        ]
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)
    elif n.startswith ("talos/gripper_left"):
        ps.createLockedJoint(n, n, half_sitting[r:r+s])
# 3}}}

# Generate constraints {{{3
graph = ConstraintGraph (robot, 'graph')
robot.setCurrentConfig (q_init)
leftpos = robot.getJointPosition (robot.leftAnkle)
rightpos = robot.getJointPosition (robot.rightAnkle)

leftfoot = [ [
# "leftfoot/comz" ,
"leftfoot/com", "leftfoot/z" , "leftfoot/rxry"],
             ["leftfoot/xy",
               ]]
               # "leftfoot/rxryrz"]]
ps.createRelativeComConstraint (leftfoot[0][0], "", robot.leftAnkle, (0,0,0.7028490491159864), (True,True,True))
ps.createPositionConstraint (leftfoot[0][1], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (False, False, True))
ps.createPositionConstraint (leftfoot[1][0], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (True, True, False))
ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][0], False)
ps.createOrientationConstraint (leftfoot[0][2], robot.leftAnkle, "", (1,0,0,0), (True, True, True))
# ps.createOrientationConstraint (leftfoot[1][2], robot.leftAnkle, "", (1,0,0,0), (False, False, True))
# ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][2], True)

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))