def get_tool_link(robot):
robot_name = get_body_name(robot)
if robot_name == FRANKA_CARTER:
return FRANKA_TOOL_LINK
#elif robot_name == EVE:
# return EVE_TOOL_LINK.format(arm=DEFAULT_ARM)
raise ValueError(robot_name)
def save_inverse_reachability(robot, arm, grasp_type, tool_link,
gripper_from_base_list):
# TODO: store value of torso and roll joint for the IK database. Sample the roll joint.
# TODO: hash the pr2 urdf as well
filename = IR_FILENAME.format(grasp_type, arm)
path = get_database_file(filename)
data = {
'filename': filename,
'robot': get_body_name(robot),
'grasp_type': grasp_type,
'arm': arm,
'torso': get_group_conf(robot, 'torso'),
'carry_conf': get_carry_conf(arm, grasp_type),
'tool_link': tool_link,
'ikfast': is_ik_compiled(),
'gripper_from_base': gripper_from_base_list,
}
write_pickle(path, data)
if has_gui():
handles = []
for gripper_from_base in gripper_from_base_list:
handles.extend(
draw_point(point_from_pose(gripper_from_base),
color=(1, 0, 0)))
wait_for_user()
return path
def get_gripper_link(robot):
robot_name = get_body_name(robot)
if robot_name == FRANKA_CARTER:
return FRANKA_GRIPPER_LINK
#elif robot_name == EVE:
# #return EVE_GRIPPER_LINK.format(a='l') # TODO: issue copying *.dae
# return EVE_GRIPPER_LINK.format(arm=DEFAULT_ARM)
raise ValueError(robot_name)
def test_simulation(robot, target_x, video=None):
use_turtlebot = (get_body_name(robot) == 'turtlebot')
if not use_turtlebot:
target_point, target_quat = map(list, get_pose(robot))
target_point[0] = target_x
add_pose_constraint(robot,
pose=(target_point, target_quat),
max_force=200) # TODO: velocity constraint?
else:
# p.changeDynamics(robot, robot_joints[0], # Doesn't work
# maxJointVelocity=1,
# jointLimitForce=1,)
robot_joints = get_movable_joints(robot)[:3]
print('Max velocities:', get_max_velocities(robot, robot_joints))
print('Max forces:', get_max_forces(robot, robot_joints))
control_joint(robot,
joint=robot_joints[0],
position=target_x,
velocity=0,
position_gain=None,
velocity_scale=None,
max_velocity=100,
max_force=300)
# control_joints(robot, robot_joints, positions=[target_x, 0, PI], max_force=300)
# velocity_control_joints(robot, robot_joints, velocities=[-2., 0, 0]) #, max_force=300)
robot_link = get_first_link(robot)
if video is None:
wait_if_gui('Begin?')
simulate(controller=condition_controller(
lambda *args: abs(target_x - point_from_pose(
get_link_pose(robot, robot_link))[0]) < 1e-3),
sleep=0.01) # TODO: velocity condition
# print('Velocities:', get_joint_velocities(robot, robot_joints))
# print('Torques:', get_joint_torques(robot, robot_joints))
if video is None:
set_renderer(enable=True)
wait_if_gui('Finish?')
def create_inverse_reachability(robot, body, table, arm, grasp_type, num_samples=500):
link = get_gripper_link(robot, arm)
movable_joints = get_movable_joints(robot)
default_conf = get_joint_positions(robot, movable_joints)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
while len(gripper_from_base_list) < num_samples:
box_pose = sample_placement(body, table)
set_pose(body, box_pose)
grasp_pose = random.choice(grasps)
gripper_pose = multiply(box_pose, invert(grasp_pose))
set_joint_positions(robot, movable_joints, default_conf)
base_conf = next(uniform_pose_generator(robot, gripper_pose))
set_base_values(robot, base_conf)
if pairwise_collision(robot, table):
continue
conf = inverse_kinematics(robot, link, gripper_pose)
if (conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, link)), get_pose(robot))
gripper_from_base_list.append(gripper_from_base)
print('{} / {}'.format(len(gripper_from_base_list), num_samples))
filename = IR_FILENAME.format(grasp_type, arm)
path = get_database_file(filename)
data = {
'filename': filename,
'robot': get_body_name(robot),
'grasp_type': grasp_type,
'arg': arm,
'carry_conf': get_carry_conf(arm, grasp_type),
'gripper_link': link,
'gripper_from_base': gripper_from_base_list,
}
write_pickle(path, data)
return path
def collect_place(world, object_name, surface_name, grasp_type, args):
date = get_date()
#set_seed(args.seed)
#dump_body(world.robot)
surface_pose = get_surface_reference_pose(world.kitchen, surface_name) # TODO: assumes the drawer is open
stable_gen_fn = get_stable_gen(world, z_offset=Z_EPSILON, visibility=False,
learned=False, collisions=not args.cfree)
grasp_gen_fn = get_grasp_gen(world)
ik_ir_gen = get_pick_gen_fn(world, learned=False, collisions=not args.cfree, teleport=args.teleport)
stable_gen = stable_gen_fn(object_name, surface_name)
grasps = list(grasp_gen_fn(object_name, grasp_type))
robot_name = get_body_name(world.robot)
path = get_place_path(robot_name, surface_name, grasp_type)
print(SEPARATOR)
print('Robot name: {} | Object name: {} | Surface name: {} | Grasp type: {} | Filename: {}'.format(
robot_name, object_name, surface_name, grasp_type, path))
entries = []
start_time = time.time()
failures = 0
while (len(entries) < args.num_samples) and \
(elapsed_time(start_time) < args.max_time): #and (failures <= max_failures):
(rel_pose,) = next(stable_gen)
if rel_pose is None:
break
(grasp,) = random.choice(grasps)
with LockRenderer(lock=True):
result = next(ik_ir_gen(object_name, rel_pose, grasp), None)
if result is None:
print('Failure! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
failures += 1
continue
# TODO: ensure an arm motion exists
bq, aq, at = result
rel_pose.assign()
bq.assign()
aq.assign()
base_pose = get_link_pose(world.robot, world.base_link)
object_pose = rel_pose.get_world_from_body()
tool_pose = multiply(object_pose, invert(grasp.grasp_pose))
entries.append({
'tool_from_base': multiply(invert(tool_pose), base_pose),
'surface_from_object': multiply(invert(surface_pose), object_pose),
'base_from_object': multiply(invert(base_pose), object_pose),
})
print('Success! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
if has_gui():
wait_for_user()
#visualize_database(tool_from_base_list)
if not entries:
safe_remove(path)
return None
# Assuming the kitchen is fixed but the objects might be open world
data = {
'date': date,
'robot_name': robot_name, # get_name | get_body_name | get_base_name | world.robot_name
'base_link': get_link_name(world.robot, world.base_link),
'tool_link': get_link_name(world.robot, world.tool_link),
'kitchen_name': get_body_name(world.kitchen),
'surface_name': surface_name,
'object_name': object_name,
'grasp_type': grasp_type,
'entries': entries,
'failures': failures,
'successes': len(entries),
}
write_json(path, data)
print('Saved', path)
return data
def main():
# TODO: teleporting kuka arm
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
client = connect(use_gui=args.viewer)
add_data_path()
#planeId = p.loadURDF("plane.urdf")
table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
box = create_box(.07, .05, .15)
# boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
#pr2 = p.loadURDF("pr2_description/pr2.urdf")
pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf")
#pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",)
#useFixedBase=0,)
#flags=p.URDF_USE_SELF_COLLISION)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
#pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False)
initially_colliding = get_colliding_links(pr2)
print len(initially_colliding)
origin = (0, 0, 0)
print p.getNumConstraints()
# TODO: no way of controlling the base position by itself
# TODO: PR2 seems to collide with itself frequently
# real_time = False
# p.setRealTimeSimulation(real_time)
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# while True:
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# if not real_time:
# p.stepSimulation()
# A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints.
# GetRigidlyAttachedLinks
print pr2
# for i in range (10000):
# p.stepSimulation()
# time.sleep(1./240.)
#print get_joint_names(pr2)
print[get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)]
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER))
print self_collision(pr2)
"""
print p.getNumConstraints()
constraint = fixed_constraint(pr2, -1, box, -1) # table
p.changeConstraint(constraint)
print p.getNumConstraints()
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
p.stepSimulation()
raw_input('Continue?')
set_point(pr2, (-2, 0, 0))
p.stepSimulation()
p.changeConstraint(constraint)
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
raw_input('Continue?')
print get_point(pr2)
raw_input('Continue?')
"""
# TODO: would be good if we could set the joint directly
print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME),
0.2) # Updates automatically
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#return
left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES]
print set_joint_positions(
pr2, left_joints,
TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
print set_joint_positions(
pr2, right_joints,
REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM
print get_body_name(pr2)
print get_body_names()
# print p.getBodyUniqueId(pr2)
print get_joint_names(pr2)
#for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM):
# set_joint_position(pr2, joint, value)
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# #print name, joint, get_joint_position(pr2, joint), value
# print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint)
# set_joint_position(pr2, joint, value)
# #print name, joint, get_joint_position(pr2, joint), value
# for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM):
# set_joint_position(pr2, joint_from_name(pr2, name), value)
print p.getNumJoints(pr2)
jointId = 0
print p.getJointInfo(pr2, jointId)
print p.getJointState(pr2, jointId)
# for i in xrange(10):
# #lower, upper = BASE_LIMITS
# #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower
# q = np.random.uniform(*BASE_LIMITS)
# theta = np.random.uniform(*REVOLUTE_LIMITS)
# quat = z_rotation(theta)
# print q, theta, quat, env_collision(pr2)
# #set_point(pr2, q)
# set_pose(pr2, q, quat)
# #p.getMouseEvents()
# #p.getKeyboardEvents()
# raw_input('Continue?') # Stalls because waiting for input
#
# # TODO: self collisions
# for i in xrange(10):
# for name in LEFT_JOINT_NAMES:
# joint = joint_from_name(pr2, name)
# value = np.random.uniform(*get_joint_limits(pr2, joint))
# set_joint_position(pr2, joint, value)
# raw_input('Continue?')
#start = (-2, -2, 0)
#set_base_values(pr2, start)
# #start = get_base_values(pr2)
# goal = (2, 2, 0)
# p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText
# print start, goal
# raw_input('Plan?')
# path = plan_base_motion(pr2, goal)
# print path
# if path is None:
# return
# print len(path)
# for bq in path:
# set_base_values(pr2, bq)
# raw_input('Continue?')
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# for joint in left_joints:
# print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \
# is_circular(pr2, joint), get_joint_position(pr2, joint)
#
# #goal = np.zeros(len(left_joints))
# goal = []
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# goal.append(wrap_joint(pr2, joint, value))
#
# path = plan_joint_motion(pr2, left_joints, goal)
# print path
# for q in path:s
# set_joint_positions(pr2, left_joints, q)
# raw_input('Continue?')
print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6
movable_joints = get_movable_joints(pr2)
print len(movable_joints)
for joint in xrange(get_num_joints(pr2)):
if is_movable(pr2, joint):
print joint, get_joint_name(pr2, joint), get_joint_type(
pr2, joint), get_joint_limits(pr2, joint)
#joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
#set_joint_positions(pr2, joints, sample_joints(pr2, joints))
#print get_joint_positions(pr2, joints) # Need to print before the display updates?
# set_base_values(pr2, (1, -1, -np.pi/4))
# movable_joints = get_movable_joints(pr2)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# print gripper_pose
# print get_joint_positions(pr2, movable_joints)
# p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0))
# p.stepSimulation()
# raw_input('Pre2 IK')
# set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
# print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Pre IK')
# conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons
# print conf
# print get_joint_positions(pr2, movable_joints)
# set_joint_positions(pr2, movable_joints, conf)
# print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Post IK')
# return
# print pose_from_tform(TOOL_TFORM)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #gripper_pose = multiply(gripper_pose, TOOL_POSE)
# #gripper_pose = get_gripper_pose(pr2)
# for i, grasp_pose in enumerate(get_top_grasps(box)):
# grasp_pose = multiply(TOOL_POSE, grasp_pose)
# box_pose = multiply(gripper_pose, grasp_pose)
# set_pose(box, *box_pose)
# print get_pose(box)
# raw_input('Grasp {}'.format(i))
# return
torso = joint_from_name(pr2, TORSO_JOINT_NAME)
torso_point, torso_quat = get_link_pose(pr2, torso)
#torso_constraint = p.createConstraint(pr2, torso, -1, -1,
# p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED
# parentFramePosition=torso_point,
# childFramePosition=torso_quat)
create_inverse_reachability(pr2, box, table)
ir_database = load_inverse_reachability()
print len(ir_database)
return
link = link_from_name(pr2, LEFT_ARM_LINK)
point, quat = get_link_pose(pr2, link)
print point, quat
p.addUserDebugLine(origin, point,
lineColorRGB=(1, 1, 0)) # addUserDebugText
raw_input('Continue?')
current_conf = get_joint_positions(pr2, movable_joints)
#ik_conf = p.calculateInverseKinematics(pr2, link, point)
#ik_conf = p.calculateInverseKinematics(pr2, link, point, quat)
min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints]
max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints]
max_velocities = [
get_max_velocity(pr2, joint) for joint in movable_joints
] # Range of Jacobian
print min_limits
print max_limits
print max_velocities
ik_conf = p.calculateInverseKinematics(pr2,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf)
value_from_joint = dict(zip(movable_joints, ik_conf))
print[value_from_joint[joint] for joint in joints]
#print len(ik_conf), ik_conf
set_joint_positions(pr2, movable_joints, ik_conf)
#print len(movable_joints), get_joint_positions(pr2, movable_joints)
print get_joint_positions(pr2, joints)
raw_input('Finish?')
p.disconnect()
def ss_from_problem(robot,
movable=[],
bound='shared',
teleport=False,
movable_collisions=False,
grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
rigid = [body for body in get_bodies() if body != robot]
fixed = [body for body in rigid if body not in movable]
print('Robot:', robot)
print('Movable:', movable)
print('Fixed:', fixed)
conf = BodyConf(robot, get_configuration(robot))
initial_atoms = [
HandEmpty(),
CanMove(),
IsConf(conf),
AtConf(conf),
initialize(TotalCost(), 0),
]
for body in movable:
pose = BodyPose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose)
]
for surface in fixed:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
initial_atoms.append(Sink(body))
if 'stove' in name:
initial_atoms.append(Stove(body))
body = movable[0]
goal_literals = [
AtConf(conf),
#Holding(body),
#On(body, fixed[0]),
#On(body, fixed[2]),
#Cleaned(body),
Cooked(body),
]
PlacedCollision = Predicate([T, O, P],
domain=[IsTraj(T), IsPose(O, P)],
fn=get_movable_collision_test(),
bound=False)
actions = [
Action(name='pick',
param=[O, P, G, Q, T],
pre=[
IsKin(O, P, G, Q, T),
HandEmpty(),
AtPose(O, P),
AtConf(Q), ~Unsafe(T)
],
eff=[HasGrasp(O, G),
CanMove(), ~HandEmpty(), ~AtPose(O, P)]),
Action(
name='place',
param=[O, P, G, Q, T],
pre=[IsKin(O, P, G, Q, T),
HasGrasp(O, G),
AtConf(Q), ~Unsafe(T)],
eff=[HandEmpty(),
CanMove(),
AtPose(O, P), ~HasGrasp(O, G)]),
# Can just do move if we check holding collisions
Action(name='move_free',
param=[Q, Q2, T],
pre=[
IsFreeMotion(Q, Q2, T),
HandEmpty(),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(name='move_holding',
param=[Q, Q2, O, G, T],
pre=[
IsHoldingMotion(Q, Q2, O, G, T),
HasGrasp(O, G),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Sink(O2), ~Cooked(O),
On(O, O2)],
eff=[Cleaned(O)]),
Action(
name='cook',
param=[O, O2], # Wirelessly communicates to cook
pre=[Stackable(O, O2),
Stove(O2), Cleaned(O),
On(O, O2)],
eff=[Cooked(O), ~Cleaned(O)]),
]
axioms = [
Axiom(param=[O, G],
pre=[IsGrasp(O, G), HasGrasp(O, G)],
eff=Holding(O)),
Axiom(param=[O, P, O2],
pre=[IsPose(O, P),
IsSupported(P, O2),
AtPose(O, P)],
eff=On(O, O2)),
]
if movable_collisions:
axioms += [
Axiom(param=[T, O, P],
pre=[IsPose(O, P),
PlacedCollision(T, O, P),
AtPose(O, P)],
eff=Unsafe(T)),
]
streams = [
GenStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(robot, grasp_name),
out=[G],
graph=[IsGrasp(O, G)],
bound=bound),
# TODO: test_support
GenStream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(fixed=fixed),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2)],
bound=bound),
FnStream(name='inverse_kin',
inp=[O, P, G],
domain=[IsPose(O, P), IsGrasp(O, G)],
fn=get_ik_fn(robot, fixed=fixed, teleport=teleport),
out=[Q, T],
graph=[IsKin(O, P, G, Q, T),
IsConf(Q), IsTraj(T)],
bound=bound),
FnStream(name='free_motion',
inp=[Q, Q2],
domain=[IsConf(Q), IsConf(Q2)],
fn=get_free_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsFreeMotion(Q, Q2, T),
IsTraj(T)],
bound=bound),
FnStream(name='holding_motion',
inp=[Q, Q2, O, G],
domain=[IsConf(Q), IsConf(Q2),
IsGrasp(O, G)],
fn=get_holding_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsHoldingMotion(Q, Q2, O, G, T),
IsTraj(T)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
def collect_pull(world, joint_name, args):
date = get_date()
#set_seed(args.seed)
robot_name = get_body_name(world.robot)
if is_press(joint_name):
press_gen = get_press_gen_fn(world,
collisions=not args.cfree,
teleport=args.teleport,
learned=False)
else:
joint = joint_from_name(world.kitchen, joint_name)
open_conf = Conf(world.kitchen, [joint], [world.open_conf(joint)])
closed_conf = Conf(world.kitchen, [joint], [world.closed_conf(joint)])
pull_gen = get_pull_gen_fn(world,
collisions=not args.cfree,
teleport=args.teleport,
learned=False)
#handle_link, handle_grasp, _ = get_handle_grasp(world, joint)
path = get_pull_path(robot_name, joint_name)
print(SEPARATOR)
print('Robot name {} | Joint name: {} | Filename: {}'.format(
robot_name, joint_name, path))
entries = []
failures = 0
start_time = time.time()
while (len(entries) < args.num_samples) and \
(elapsed_time(start_time) < args.max_time):
if is_press(joint_name):
result = next(press_gen(joint_name), None)
else:
result = next(pull_gen(joint_name, open_conf, closed_conf),
None) # Open to closed
if result is None:
print('Failure! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
failures += 1
continue
if not is_press(joint_name):
open_conf.assign()
joint_pose = get_joint_reference_pose(world.kitchen, joint_name)
bq, aq1 = result[:2]
bq.assign()
aq1.assign()
#next(at.commands[2].iterate(None, None))
base_pose = get_link_pose(world.robot, world.base_link)
#handle_pose = get_link_pose(world.robot, base_link)
entries.append({
'joint_from_base':
multiply(invert(joint_pose), base_pose),
})
print('Success! | {} / {} [{:.3f}]'.format(len(entries),
args.num_samples,
elapsed_time(start_time)))
if has_gui():
wait_for_user()
if not entries:
safe_remove(path)
return None
#visualize_database(joint_from_base_list)
# Assuming the kitchen is fixed but the objects might be open world
# TODO: could store per data point
data = {
'date': date,
'robot_name':
robot_name, # get_name | get_body_name | get_base_name | world.robot_name
'base_link': get_link_name(world.robot, world.base_link),
'tool_link': get_link_name(world.robot, world.tool_link),
'kitchen_name': get_body_name(world.kitchen),
'joint_name': joint_name,
'entries': entries,
'failures': failures,
'successes': len(entries),
}
if not is_press(joint_name):
data.update({
'open_conf': open_conf.values,
'closed_conf': closed_conf.values,
})
write_json(path, data)
print('Saved', path)
return data