if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
ps.addPassiveDofs ('pr2', jointNames ['pr2'])
cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', 'pr2')
cg.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2', 'pr2')
cg.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2')
lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock')
locklhand = ['l_l_finger','l_r_finger'];
ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5])
lockrhand = ['r_l_finger','r_r_finger'];
ps.createLockedJoint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', [0.5])
ps.createLockedJoint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', [0.5])
lockhands = lockrhand + locklhand
lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'];
ps.createLockedJoint ('head_pan', 'pr2/head_pan_joint',
[q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])
ps.createLockedJoint ('head_tilt', 'pr2/head_tilt_joint',
[q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])
ps.createLockedJoint ('torso', 'pr2/torso_lift_joint',
[q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])
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]))
ljs = list()
ps.createLockedJoint("tiago_base", "tiago/root_joint", [0,0,1,0])
lockJoint("part/root_joint", q0, "lock_part", constantRhs=False)
c_lock_part = ps.hppcorba.problem.getConstraint("lock_part")
for n in robot.jointNames:
if n.startswith('tiago/gripper_') or n.startswith('tiago/hand_'):
ljs.append(lockJoint(n, q0))
lock_arm = [ lockJoint(n, q0) for n in robot.jointNames
if n.startswith("tiago/arm") or n.startswith('tiago/torso')]
lock_head = [ lockJoint(n, q0) for n in robot.jointNames
if n.startswith("tiago/head")]
# Create "Look at gripper" constraints: for (X,Y,Z) the position of the gripper in the camera frame,
# (X, Y) = 0 and Z >= 0
rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint']
q_goal[rank] = -0.5
rank = robot.rankInConfiguration['box/root_joint']
q_goal[rank:rank + 7] = [-4.8, -4.8, 0.9, 0, 0, 1, 0]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
#q_goal [rank:rank+4] = [0, 0, 0, 1]
# 2}}}
# Build rules
rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), Rule([""], [""], True)]
locklhand = ['l_l_finger', 'l_r_finger']
ps.createLockedJoint('l_l_finger', 'pr2/l_gripper_l_finger_joint', [
0.5,
])
ps.createLockedJoint('l_r_finger', 'pr2/l_gripper_r_finger_joint', [
0.5,
])
grippers = ['pr2/l_gripper']
boxes = ['box']
handlesPerObject = [['box/handle']]
# envSurfaces = ['kitchen_area/pancake_table_table_top',
# 'kitchen_area/white_counter_top_sink']
# contactPerObject = [['box/box_surface']]
envSurfaces = []
contactPerObject = [[]]
ps.client.manipulation.graph.autoBuild('graph', grippers, boxes,
-2.,
2,
-2.,
2,
])
## Lock both hands
locklhand = list()
for j, v in robot.leftHandOpen.iteritems():
locklhand.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)
lockrhand = list()
for j, v in robot.rightHandOpen.iteritems():
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
srdfSuffix = ""
robot = Robot ('tom-box', 'tom')
robot.setJointBounds ('tom/base_joint_xy', [-1,1,-1,1])
ps = ProblemSolver (robot)
fk = ViewerFactory (ps)
fk.loadObjectModel (Box, 'box')
robot.setJointBounds ('box/base_joint_xyz', [-1,1,-1,1,-1,1])
openLeftHand = []
for j, v in robot.openHand (None, .75, "left").iteritems () :
lockedJointName = "open/"+j
openLeftHand.append (lockedJointName)
ps.createLockedJoint (lockedJointName, j, [v,])
openRightHand = []
for j, v in robot.openHand (None, .75, "right").iteritems () :
lockedJointName = "open/"+j
openRightHand.append (lockedJointName)
ps.createLockedJoint (lockedJointName, j, [v,])
closeLeftHand = []
for j, v in robot.openHand (None, .24, "left").iteritems () :
lockedJointName = "close/"+j
closeLeftHand.append (lockedJointName)
ps.createLockedJoint (lockedJointName, j, [v,])
closeRightHand = []
for j, v in robot.openHand (None, .24, "right").iteritems () :
robot.client.manipulation.problem.createPrePlacementConstraint (
'pre_box2_on_table', ['box2/box_surface'], ['table/pancake_table_table_top'], 0.05)
robot.client.manipulation.problem.createPlacementConstraint (
'box2_on_box1', ['box1/box_surface'], ['box2/box_surface'])
robot.client.manipulation.problem.createPrePlacementConstraint (
'pre_box2_on_box1', ['box1/box_surface'], ['box2/box_surface'], 0.05)
# 4}}}
# Locks joints that are not used for this problem {{{4
lockFingers = ["r_gripper_l_finger",
"r_gripper_r_finger",
"l_gripper_l_finger",
"l_gripper_r_finger",
]
for side in ["r", "l", ]:
ps.createLockedJoint(side + "_gripper_l_finger", "baxter/" + side + "_gripper_l_finger_joint", [ 0.02,])
ps.createLockedJoint(side + "_gripper_r_finger", "baxter/" + side + "_gripper_r_finger_joint", [-0.02,])
lockHead = ['head_pan',]
ps.createLockedJoint ('head_pan', 'baxter/head_pan',
[q_init[robot.rankInConfiguration['baxter/head_pan']]])
for n in jointNames["baxterRightSide"]:
ps.createLockedJoint (n, n, [0,])
for n in jointNames["baxterLeftSide"]:
ps.createLockedJoint (n, n, [0,])
lockAll = lockFingers + lockHead + jointNames["baxterLeftSide"]
# 4}}}
['kitchen_area/pancake_table_table_top',])
jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['pr2'] = list()
jointNames['allButPR2LeftArm'] = list()
for n in jointNames['all']:
if n.startswith("pr2"):
jointNames['pr2'].append(n)
if not n.startswith("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append(n)
ps.addPassiveDofs('pr2', jointNames['pr2'])
locklhand = ['l_l_finger', 'l_r_finger']
ps.createLockedJoint('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5])
ps.createLockedJoint('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5])
lockPlanar = ps.lockPlanarJoint("pr2/root_joint", "lockplanar",
[-3.2, -4, 1, 0])
lockrhand = ['r_l_finger', 'r_r_finger']
ps.createLockedJoint('r_l_finger', 'pr2/r_gripper_l_finger_joint', [0.5])
ps.createLockedJoint('r_r_finger', 'pr2/r_gripper_r_finger_joint', [0.5])
lockhands = lockrhand + locklhand
lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser']
ps.createLockedJoint('head_pan', 'pr2/head_pan_joint',
[q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])
ps.createLockedJoint(
'head_tilt', 'pr2/head_tilt_joint',
robot.setJointBounds ('romeo/root_joint' , [-1,1,-1,1, 0, 2,-2.,2,-2.,2,-2.,2,
-2.,2,])
placard = Placard ('placard', vf)
robot.setJointBounds (placard.name + '/root_joint', [-1,1,-1,1,0,1.5,-2.,2,
-2.,2,-2.,2,-2.,2,])
## Lock both hands
locklhand = list()
for j,v in robot.leftHandOpen.iteritems():
locklhand.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)
lockrhand = list()
for j,v in robot.rightHandOpen.iteritems():
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
class Calibration(object):
def __init__(self, client):
self.client = client
self.robot = None
self.robots = list()
self.robot_locks = list()
self.robots_locks = list()
self.robots_gaze = list()
self.robots_identity_constraints = list()
self.q = []
def initRobot(self):
# The first robot is loaded differently by hpp
robot_id = len(self.robots) + int(self.robot is not None)
robot_name = "talos_" + str(robot_id)
if self.robot is None:
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
self.robot = Robot(
"talos", robot_name, rootJointType="freeflyer", client=self.client
)
self.ps = ProblemSolver(self.robot)
self.ps.setErrorThreshold(0.00000001)
self.ps.setMaxIterProjection(100)
self.vf = ViewerFactory(self.ps)
else:
self.robots.append(OtherRobot(name=robot_name, vf=self.vf))
self.robot.setJointBounds(robot_name + "/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
rank, size = self.getRankAndConfigSize(robot_id)
self.q[rank : rank + size] = self.robot.getCurrentConfig()[rank : rank + size]
return robot_id
def setLockJoints(self, robot_id, joints_name_value_tuple):
self.ps.createLockedJoint(
"talos_" + str(robot_id) + "/root_joint",
"talos_" + str(robot_id) + "/root_joint",
[0, 0, 1, 0, 0, 0, 1],
)
root_joint_rank = self.robot.rankInConfiguration[
"talos_" + str(robot_id) + "/root_joint"
]
self.q[root_joint_rank : root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1]
self.robots_locks.append("talos_" + str(robot_id) + "/root_joint")
# self.ps.createLockedJoint("talos_" + str(robot_id) + "/calib_mire_joint_2", "talos_" + str(robot_id) + "/calib_mire_joint_2", [0, 0, 0, 0, 0, 0, 1])
# mire_joint_rank = self.robot.rankInConfiguration["talos_" + str(robot_id) + "/calib_mire_joint_2"]
# self.q[mire_joint_rank:mire_joint_rank + 7] = [0, 0, 0, 0, 0, 0, 1]
# self.robots_locks.append("talos_" + str(robot_id) + "/calib_mire_joint_2")
for name, value in joints_name_value_tuple:
joint_name = "talos_" + str(robot_id) + "/" + name
self.q[self.robot.rankInConfiguration[joint_name]] = value
self.ps.createLockedJoint(joint_name, joint_name, [value])
self.robots_locks.append(joint_name)
def getRankAndConfigSize(self, robot_id):
robot_name = "talos_" + str(robot_id)
rank = self.robot.rankInConfiguration[robot_name + "/root_joint"]
size = sum(
[
self.robot.getJointConfigSize(joint_name)
for joint_name in calib.robot.getJointNames()
if robot_name in joint_name
]
)
return rank, size
def setGaze(self, robot_id, checkerboard_pose):
robot_name = "talos_" + str(robot_id)
position = (
checkerboard_pose.position.x,
checkerboard_pose.position.y,
checkerboard_pose.position.z,
)
orientation = (
checkerboard_pose.orientation.x,
checkerboard_pose.orientation.y,
checkerboard_pose.orientation.z,
checkerboard_pose.orientation.w,
)
self.ps.createPositionConstraint(
robot_name + "gaze",
robot_name + "/calib_rgb_joint",
robot_name + "/calib_mire_joint_2",
position,
(0, 0, 0),
[True, True, True],
)
self.ps.createOrientationConstraint(
robot_name + "gaze_O",
robot_name + "/calib_mire_joint_2",
robot_name + "/calib_rgb_joint",
orientation,
[True, True, True],
)
self.robots_gaze.append(robot_name + "gaze")
self.robots_gaze.append(robot_name + "gaze_O")
def constrainFreeflyers(self):
for robot_id in range(1, len(self.robots)):
robot_name = "talos_" + str(robot_id)
self.client.basic.problem.createIdentityConstraint(
robot_name + "_id_rgb",
["talos_0/calib_rgb_joint"],
[robot_name + "/calib_rgb_joint"],
)
self.client.basic.problem.createIdentityConstraint(
robot_name + "_id_mire",
["talos_0/calib_mire_joint_2"],
[robot_name + "/calib_mire_joint_2"],
)
self.robots_identity_constraints.append(robot_name + "_id_rgb")
self.robots_identity_constraints.append(robot_name + "_id_mire")
def optimize(self, q_init, robots_id):
self.q_proj = self.q
client.basic.problem.resetConstraints()
calib.robot.setCurrentConfig(q_init)
client.basic.problem.addLockedJointConstraints("unused1", calib.robots_locks)
gaze = [
c
for c in calib.robots_gaze
if any(["talos_" + str(robot_id) in c for robot_id in robots_id])
]
num_constraints = gaze + calib.robots_identity_constraints
client.basic.problem.addNumericalConstraints(
"unused2", num_constraints, [0 for _ in num_constraints]
)
# client.basic.problem.addNumericalConstraints("mighty_cam_cost", ["cam_cost"], [ 1 ])
# calib.client.basic.problem.setNumericalConstraintsLastPriorityOptional(True)
projOk, self.q_proj, error = client.basic.problem.applyConstraints(q_init)
if error < 1.0:
optOk, self.q_proj, error = client.basic.problem.optimize(self.q_proj)
if optOk:
print("All was good! " + str(max(error)))
else:
print("Optimisation error: " + str(max(error)))
else:
print("Projection error: " + str(error))
rank_rgb = calib.robot.rankInConfiguration["talos_0/calib_rgb_joint"]
rank_mire = calib.robot.rankInConfiguration["talos_0/calib_mire_joint_2"]
return (
self.q_proj,
self.q_proj[rank_rgb : rank_rgb + 7],
max(error),
self.q_proj[rank_mire : rank_mire + 7],
)
# cg.createGrasp ('r_fridge_grasp' , 'pr2/r_gripper', Kitchen.handle, 'pr2WithoutRightArm')
# cg.createPreGrasp ('r_fridge_pregrasp', 'pr2/r_gripper', Kitchen.handle, 'pr2WithoutRightArm')
cg.createGrasp ('r_fridge_grasp' , 'romeo/r_gripper', Kitchen.handle)
cg.createPreGrasp ('r_fridge_pregrasp', 'romeo/r_gripper', Kitchen.handle)
# cg.createGrasp ('l_cup_grasp' , 'pr2/l_gripper', Cup.handle, 'pr2WithoutLeftArm')
# cg.createPreGrasp ('l_cup_pregrasp', 'pr2/l_gripper', Cup.handle, 'pr2WithoutLeftArm')
cg.createGrasp ('l_cup_grasp' , 'romeo/l_gripper', Cup.handle, 'romeo')
cg.createPreGrasp ('l_cup_pregrasp', 'romeo/l_gripper', Cup.handle, 'romeo')
# 4}}}
# Locks joints that are not used for this problem {{{4
lockKitchen = list ()
for n in jointNames["kitchen_area"]:
if not n == Kitchen.joint:
ps.createLockedJoint (n, n, [0,])
lockKitchen.append (n)
locklhand = list()
for j,v in robot.leftHandOpen.iteritems():
locklhand.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)
lockrhand = list()
for j,v in robot.rightHandOpen.iteritems():
locklhand.append ('romeo/' + j)
if type(v) is float or type(v) is int:
q_init[rank : rank + 3] = [-2.5, -4, 0.746]
# Put box in right orientation
q_init[rank + 3 : rank + 7] = [0, -sqrt(2) / 2, 0, sqrt(2) / 2]
q_goal = q_init[::]
q_goal[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration["box/root_joint"]
q_goal[rank : rank + 3] = [-2.5, -4.5, 0.746]
# Create the constraints.
locklhand = ["l_l_finger", "l_r_finger"]
ps.createLockedJoint(
"l_l_finger",
"pr2/l_gripper_l_finger_joint",
[
0.5,
],
)
ps.createLockedJoint(
"l_r_finger",
"pr2/l_gripper_r_finger_joint",
[
0.5,
],
)
# Create the constraint graph.
# Define the set of grippers used for manipulation
grippers = [
"pr2/l_gripper",
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],])
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.createLockedJoint ('rarm_6' , 'hrp2/RARM_JOINT6' , [q_init[irh],])
ps.createLockedJoint ('rhand_0', 'hrp2/RHAND_JOINT0', [q_init[irh + 1],])
ps.createLockedJoint ('rhand_1', 'hrp2/RHAND_JOINT1', [q_init[irh + 2],])
ps.createLockedJoint ('rhand_2', 'hrp2/RHAND_JOINT2', [q_init[irh + 3],])
ps.createLockedJoint ('rhand_3', 'hrp2/RHAND_JOINT3', [q_init[irh + 4],])
ps.createLockedJoint ('rhand_4', 'hrp2/RHAND_JOINT4', [q_init[irh + 5],])
# cg.createPreGrasp ('r_fridge_pregrasp', 'pr2/r_gripper', Kitchen.handle, 'pr2WithoutRightArm')
cg.createGrasp('r_fridge_grasp', 'romeo/r_gripper', Kitchen.handle)
cg.createPreGrasp('r_fridge_pregrasp', 'romeo/r_gripper', Kitchen.handle)
# cg.createGrasp ('l_cup_grasp' , 'pr2/l_gripper', Cup.handle, 'pr2WithoutLeftArm')
# cg.createPreGrasp ('l_cup_pregrasp', 'pr2/l_gripper', Cup.handle, 'pr2WithoutLeftArm')
cg.createGrasp('l_cup_grasp', 'romeo/l_gripper', Cup.handle, 'romeo')
cg.createPreGrasp('l_cup_pregrasp', 'romeo/l_gripper', Cup.handle, 'romeo')
# 4}}}
# Locks joints that are not used for this problem {{{4
lockKitchen = list()
for n in jointNames["kitchen_area"]:
if not n == Kitchen.joint:
ps.createLockedJoint(n, n, [
0,
])
lockKitchen.append(n)
locklhand = list()
for j, v in robot.leftHandOpen.iteritems():
locklhand.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)
lockrhand = list()
jointNames['hrp2'] = list ()
jointNames['allButHRP2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("hrp2"):
jointNames['hrp2'].append (n)
if not n.startswith ("hrp2/LARM"):
jointNames['allButHRP2LeftArm'].append (n)
ps.addPassiveDofs ('hrp2', jointNames ['hrp2'])
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', 'hrp2')
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock')
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}}}
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])
left_gripper_lock.append(n)
elif n in other_lock:
ps.createLockedJoint(n, n, half_sitting[r:r+s])
graph = ConstraintGraph.buildGenericGraph(robot, 'graph',
[ "talos/left_gripper", "talos/right_gripper", ],
[ "box", ],
[ Object.handles, ],
[ [ ], ],
[ ],
[ Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True),
Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ]
q[5] = math.sin(0.5 * (pi/2))
q[6] = math.cos(0.5 * (pi/2))
# hips offset
q[9] = q[15] = -0.7
# knees offset
q[10] = q[16] = 1.4
# ankles offset
q[11] = q[17] = -0.7
# arms offset
q[24] -= 0.5
q[38] -= 0.5
v(q)
# lock gripper right open
ps.createLockedJoint("gripper_r_lock_idj", "talos/gripper_right_inner_double_joint", [0])
ps.createLockedJoint("gripper_r_lock_f1j", "talos/gripper_right_fingertip_1_joint", [0])
ps.createLockedJoint("gripper_r_lock_f2j", "talos/gripper_right_fingertip_2_joint", [0])
ps.createLockedJoint("gripper_r_lock_isj", "talos/gripper_right_inner_single_joint", [0])
ps.createLockedJoint("gripper_r_lock_f3j", "talos/gripper_right_fingertip_3_joint", [0])
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.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],
])
ps.createLockedJoint('lhand_1', 'hrp2/LHAND_JOINT1', [
q_init[ilh + 2],
])
ps.createLockedJoint('lhand_2', 'hrp2/LHAND_JOINT2', [
q_goal[rank] = -PI
rank = robot.rankInConfiguration['ur5/elbow_joint']
q_goal[rank] = PI / 2
# 3}}}
robot.client.basic.problem.resetRoadmap()
robot.client.basic.problem.selectPathOptimizer('None')
robot.client.basic.problem.setErrorThreshold(1e-3)
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))
['table/pancake_table_table_top',], 0.05)
robot.client.manipulation.problem.createPlacementConstraint (
'box2_on_table', ['box2/box_surface',], ['table/pancake_table_table_top',])
robot.client.manipulation.problem.createPrePlacementConstraint (
'box2_above_table', ['box2/box_surface',],
['table/pancake_table_table_top',], 0.05)
# 4}}}
# Locks joints that are not used for this problem {{{4
lockFingers = ["r_gripper_l_finger",
"r_gripper_r_finger",
"l_gripper_l_finger",
"l_gripper_r_finger",
]
for side in ["r", "l", ]:
ps.createLockedJoint(side + "_gripper_l_finger", "baxter/" + side + "_gripper_l_finger_joint", [ 0.02,])
ps.createLockedJoint(side + "_gripper_r_finger", "baxter/" + side + "_gripper_r_finger_joint", [-0.02,])
lockHead = ['head_pan',]
ps.createLockedJoint ('head_pan', 'baxter/head_pan',
[q_init[robot.rankInConfiguration['baxter/head_pan']]])
for n in jointNames["baxterRightSide"]:
ps.createLockedJoint (n, n, [0,])
for n in jointNames["baxterLeftSide"]:
ps.createLockedJoint (n, n, [0,])
lockHeadFingersAndLeftSide = lockFingers + lockHead + jointNames["baxterLeftSide"]
# 4}}}
q_goal = q_init [:]
rank = robot.rankInConfiguration ['ur5/shoulder_lift_joint']
q_goal [rank] = - PI
rank = robot.rankInConfiguration ['ur5/elbow_joint']
q_goal [rank] = PI / 2
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
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))
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_goal [rank:rank+3] = [-4.8, -4.6, 0.9]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_goal [rank:rank+4] = [0, 0, 0, 1]
# 2}}}
# Create the constraints. {{{2
graph.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', passiveJoints = 'pr2')
graph.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock', compType = 'Equality')
lockpr2 = ps.lockPlanarJoint ('pr2/base_joint', 'pr2_lock', compType = 'Equality')
lockboth = lockpr2[:]; lockboth.extend (lockbox)
locklhand = ['l_l_finger','l_r_finger'];
ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,])
# 2}}}
# Create the constraint graph. {{{2
# Create nodes and edges. {{{3
graph.createNode (['box', 'free'])
we = dict ()
we["ungrasp"] = graph.createWaypointEdge ('box', 'free', "ungrasp", nb=1, weight=1)
we[ "grasp"] = graph.createWaypointEdge ('free', 'box', "grasp", nb=1, weight=10)
graph.createEdge ('free', 'free', 'move_free', 1)
graph.createEdge ('box', 'box', 'keep_grasp', 5)
graph.createLevelSetEdge ('box', 'box', 'keep_grasp_ls', 10)
-2.,
2,
-2.,
2,
])
## Lock both hands
locklhand = list()
for j, v in robot.leftHandOpen.items():
locklhand.append('romeo/' + j)
if type(v) is float or type(v) is int:
val = [
v,
]
else:
val = v
ps.createLockedJoint('romeo/' + j, robot.robotNames[0] + '/' + j, val)
lockrhand = list()
for j, v in robot.rightHandOpen.items():
lockrhand.append('romeo/' + j)
if type(v) is float or type(v) is int:
val = [
v,
]
else:
val = v
ps.createLockedJoint('romeo/' + j, robot.robotNames[0] + '/' + j, val)
lockHands = lockrhand + locklhand
## Create static stability constraint
robot.leftAnkle = robot.robotNames[0] + '/' + robot.leftAnkle
# placement constraint
placementName = "place_sphere{0}".format(i)
ps.createTransformationConstraint(placementName, "",
"sphere{0}/root_joint".format(i),
[0, 0, 0.02, 0, 0, 0, 1],
[False, False, True, True, True, False])
# placement complement constraint
ps.createTransformationConstraint(placementName + '/complement', "",
"sphere{0}/root_joint".format(i),
[0, 0, 0.02, 0, 0, 0, 1],
[True, True, False, False, False, True])
ps.setConstantRightHandSide(placementName + '/complement', False)
# combination of placement and complement
ps.createLockedJoint(
placementName + '/hold', "sphere{0}/root_joint".format(i),
[0, 0, 0.02, 0, 0, 0, 1],
[Equality, Equality, EqualToZero, EqualToZero, EqualToZero, Equality])
ps.registerConstraints(placementName, placementName + '/complement',
placementName + '/hold')
preplacementName = "preplace_sphere{0}".format(i)
ps.createTransformationConstraint(preplacementName, "",
"sphere{0}/root_joint".format(i),
[0, 0, 0.1, 0, 0, 0, 1],
[False, False, True, True, True, False])
# combination of pre-placement and complement
ps.createLockedJoint(
preplacementName + '/hold', "sphere{0}/root_joint".format(i),
[0, 0, 0.1, 0, 0, 0, 1],
[Equality, Equality, EqualToZero, EqualToZero, EqualToZero, Equality])
jointNames['allButHRP2LeftArm'] = list()
for n in jointNames['all']:
if n.startswith("hrp2"):
jointNames['hrp2'].append(n)
if not n.startswith("hrp2/LARM"):
jointNames['allButHRP2LeftArm'].append(n)
ps.addPassiveDofs('hrp2', jointNames['hrp2'])
graph.createGrasp('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', 'hrp2')
graph.createPreGrasp('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
lockscrewgun = ps.lockFreeFlyerJoint('screw_gun/base_joint', 'screwgun_lock')
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}}}
