def closed_conf(self, joint):
lower, upper = get_joint_limits(self.kitchen, joint)
if 'drawer' in get_joint_name(self.kitchen, joint):
fraction = 0.9
return fraction * lower + (1 - fraction) * upper
if 'left' in get_joint_name(self.kitchen, joint):
return upper
return lower
def main():
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
with HideOutput():
with LockRenderer():
robot = load_model(FRANKA_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_user()
tool_link = link_from_name(robot, 'panda_hand')
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
test_retraction(robot,
PANDA_INFO,
tool_link,
max_distance=0.01,
max_time=0.05)
disconnect()
def update_state(self):
# TODO: apply this directly from observations
# No use applying this to base confs
self.base_conf = FConf(self.world.robot,
self.world.base_joints,
init=True)
arm_conf = FConf(self.world.robot, self.world.arm_joints, init=True)
if (self.arm_conf is None) or not are_confs_close(
arm_conf, self.arm_conf, tol=ARM_TOLERANCE):
self.arm_conf = arm_conf
else:
print('At anticipated arm conf')
gripper_conf = FConf(self.world.robot,
self.world.gripper_joints,
init=True)
if (self.gripper_conf is None) or not are_confs_close(
gripper_conf, self.gripper_conf, tol=GRIPPER_TOLERANCE):
self.gripper_conf = gripper_conf
else:
print('At anticipated gripper conf')
# TODO: do I still need to test if the current values are equal to the last ones?
for joint in self.world.kitchen_joints:
name = get_joint_name(self.world.kitchen, joint)
position = get_joint_position(self.world.kitchen, joint)
self.update_door_conf(name, position)
self.update_door_conf(name, position)
#wait_for_user()
return self.check_consistent()
def main():
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
#with HideOutput():
with LockRenderer():
robot = load_model(MOVO_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_user()
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
disconnect()
def test_confs(robot, num_samples=10):
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(num_samples):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
def move_occluding(world):
# Prevent obstruction by other objects
# TODO: this is a bit of a hack due to pybullet
world.set_base_conf([-5.0, 0, 0])
for joint in world.kitchen_joints:
joint_name = get_joint_name(world.kitchen, joint)
if joint_name in DRAWERS:
world.open_door(joint)
else:
world.close_door(joint)
for name in world.movable:
set_pose(world.get_body(name), Pose(Point(z=-5.0)))
def open_conf(self, joint):
joint_name = get_joint_name(self.kitchen, joint)
if 'left' in joint_name:
open_position = get_min_limit(self.kitchen, joint)
else:
open_position = get_max_limit(self.kitchen, joint)
#print(get_joint_name(self.kitchen, joint), get_min_limit(self.kitchen, joint), get_max_limit(self.kitchen, joint))
# drawers: [0.0, 0.4]
# left doors: [-1.57, 0.0]
# right doors: [0.0, 1.57]
if joint_name in CABINET_JOINTS:
# TODO: could make fraction of max
return CABINET_OPEN_ANGLE * open_position / abs(open_position)
if joint_name in DRAWER_JOINTS:
return DRAWER_OPEN_FRACTION * open_position
return open_position
def main():
connect(use_gui=True)
add_data_path()
draw_pose(Pose(), length=1.)
set_camera_pose(camera_point=[1, -1, 1])
plane = p.loadURDF("plane.urdf")
with LockRenderer():
with HideOutput(True):
robot = load_pybullet(FRANKA_URDF, fixed_base=True)
assign_link_colors(robot, max_colors=3, s=0.5, v=1.)
#set_all_color(robot, GREEN)
obstacles = [plane] # TODO: collisions with the ground
dump_body(robot)
print('Start?')
wait_for_user()
info = PANDA_INFO
tool_link = link_from_name(robot, 'panda_hand')
draw_pose(Pose(), parent=robot, parent_link=tool_link)
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
check_ik_solver(info)
sample_fn = get_sample_fn(robot, joints)
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
#test_ik(robot, info, tool_link, get_link_pose(robot, tool_link))
test_retraction(robot,
info,
tool_link,
use_pybullet=False,
max_distance=0.1,
max_time=0.05,
max_candidates=100)
disconnect()
def get_door_sign(self, joint):
return -1 if 'left' in get_joint_name(self.kitchen, joint) else +1
def get_joint_names(self):
with ClientSaver(self.client):
return [
get_joint_name(self.robot, joint)
for joint in get_movable_joints(self.robot)
]
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 get_joint_names(body, joints):
return [
get_joint_name(body, joint).encode('ascii') # ,'ignore')
for joint in joints
]
