def main():
connect(use_gui=True)
with HideOutput():
pr2 = load_model(
"models/drake/pr2_description/urdf/pr2_simplified.urdf")
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']),
REST_LEFT_ARM)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']),
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']),
[0.2])
dump_body(pr2)
block = load_model(BLOCK_URDF, fixed_base=False)
set_point(block, [2, 0.5, 1])
target_point = point_from_pose(get_pose(block))
# head_link = link_from_name(pr2, HEAD_LINK)
head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
head_link = head_joints[-1]
#max_detect_distance = 2.5
max_register_distance = 1.0
distance_range = (max_register_distance / 2, max_register_distance)
base_generator = visible_base_generator(pr2, target_point, distance_range)
base_joints = joints_from_names(pr2, PR2_GROUPS['base'])
for i in range(5):
base_conf = next(base_generator)
set_joint_positions(pr2, base_joints, base_conf)
p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)),
target_point,
lineColorRGB=(1, 0, 0)) # addUserDebugText
p.addUserDebugLine(point_from_pose(
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
target_point,
lineColorRGB=(0, 0, 1)) # addUserDebugText
# head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
head_conf = inverse_visibility(pr2, target_point)
set_joint_positions(pr2, head_joints, head_conf)
print(get_detections(pr2))
# TODO: does this detect the robot sometimes?
detect_mesh, z = get_detection_cone(pr2, block)
detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5))
set_pose(detect_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25))
set_pose(view_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
wait_for_user()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()
def get_gripper_joints(robot, arm):
assert arm in ARMS
if has_kg3_gripper(robot, arm):
return joints_from_names(robot,
names_from_templates(KG3_GRIPPER_JOINTS, arm))
elif has_robotiq_gripper(robot, arm):
return joints_from_names(
robot, names_from_templates(ROBOTIQ_GRIPPER_JOINTS, arm))
raise ValueError(arm)
def main(use_pr2_drake=True):
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
table_path = "models/table_collision/table.urdf"
table = load_pybullet(table_path, fixed_base=True)
set_quat(table, quat_from_euler(Euler(yaw=PI / 2)))
# table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
obstacles = [plane, table]
pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
with HideOutput():
pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings?
dump_body(pr2)
z = base_aligned_z(pr2)
print(z)
#z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3)))
print(z)
set_point(pr2, Point(z=z))
print(get_aabb(pr2))
wait_if_gui()
base_start = (-2, -2, 0)
base_goal = (2, 2, 0)
arm_start = SIDE_HOLDING_LEFT_ARM
#arm_start = TOP_HOLDING_LEFT_ARM
#arm_start = REST_LEFT_ARM
arm_goal = TOP_HOLDING_LEFT_ARM
#arm_goal = SIDE_HOLDING_LEFT_ARM
left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
set_joint_positions(pr2, left_joints, arm_start)
set_joint_positions(pr2, right_joints,
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, torso_joints, [0.2])
open_arm(pr2, 'left')
# test_ikfast(pr2)
add_line(base_start, base_goal, color=RED)
print(base_start, base_goal)
if use_pr2_drake:
test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
else:
test_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
test_arm_motion(pr2, left_joints, arm_goal)
# test_arm_control(pr2, left_joints, arm_start)
wait_if_gui('Finish?')
disconnect()
def run_simulate(pr2):
joints = joints_from_names(pr2, PR2_GROUPS['base'])
dx = dy = dz = dth = 1
speed, turn = 0.5, 1.0
while True:
velocities = [dx * speed, dy * speed, dth * turn]
velocity_control_joints(pr2, joints, velocities)
def run_thread(pr2):
joints = joints_from_names(pr2, PR2_GROUPS['base'])
dx = dy = dz = dth = 0
speed, turn = 0.5, 1.0
settings = termios.tcgetattr(sys.stdin)
try:
print(HELP_MSG)
print_velocities(speed, turn)
while True:
key = get_key(settings) # Waits until a key is read
if key in MOVE_BINDINGS:
dx, dy, dz, dth = MOVE_BINDINGS[key]
elif key in SPEED_BINDINGS:
mspeed, mturn = SPEED_BINDINGS[key]
speed *= mspeed
turn *= mturn
print_velocities(speed, turn)
else: # When it receives another key
dx = dy = dz = dth = 0
if key == ESCAPE:
break
# twist.linear.dz = dz * speed
velocities = [dx * speed, dy * speed, dth * turn]
velocity_control_joints(pr2, joints, velocities)
except Exception as e:
print(e)
finally:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def gripper_joints(self):
#if self.robot_yaml is None:
# return []
joint_names = ['panda_finger_joint{}'.format(1 + i) for i in range(2)]
#joint_names = [joint_from_name(self.robot, rule['name'])
# for rule in self.robot_yaml['cspace_to_urdf_rules']]
return joints_from_names(self.robot, joint_names)
def kitchen_joints(self):
joint_names = get_joint_names(self.kitchen,
get_movable_joints(self.kitchen))
#joint_names = self.kitchen_yaml['cspace']
#return joints_from_names(self.kitchen, joint_names)
return joints_from_names(self.kitchen,
filter(ALL_JOINTS.__contains__, joint_names))
def execute(self, controller, joint_speed):
if not self.path:
return
# if self.teleport:
# for _ in self.iterate(controller.world, attachments={}):
# pass
arm_prefix = get_arm_prefix(self.arm)
robot = controller.world.robot
arm_joints = joints_from_names(
robot, controller.get_arm_joint_names(arm_prefix))
current_positions = controller.get_arm_positions(arm_prefix)
# For debugging
#modification = [random.randint(-10, +10) * np.pi if is_circular(robot, joint) else 0 for joint in arm_joints]
#current_positions = current_positions + np.array(modification)
difference_fn = get_difference_fn(robot, arm_joints)
differences = [
difference_fn(q2, q1) for q1, q2 in zip(self.path, self.path[1:])
]
adjusted_path = [np.array(current_positions)]
for difference in differences:
adjusted_path.append(adjusted_path[-1] + difference)
#adjusted_path = adjust_trajectory(self.path, current_positions)
#for i, (q1, q2) in enumerate(zip(self.path, trajectory)):
# print(i, (np.array(q2) - np.array(q1)).round(3).tolist())
times_from_start = generate_times(adjusted_path,
joint_speed / self.dialation)
controller.command_arm_trajectory(
arm_prefix,
adjusted_path[1:],
times_from_start=times_from_start[1:])
controller.command_arm(arm_prefix, adjusted_path[-1], timeout=1.0)
return True
def main(use_pr2_drake=False):
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
#table_path = "table/table.urdf"
# table_path = "models/table_collision/table.urdf"
# table = p.loadURDF(table_path, 0, 0, 0, 0, 0, 0.707107, 0.707107)
# table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
with HideOutput():
pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings?
dump_body(pr2)
base_start = (-2, -2, 0)
base_goal = (2, 2, 0)
arm_start = SIDE_HOLDING_LEFT_ARM
#arm_start = TOP_HOLDING_LEFT_ARM
#arm_start = REST_LEFT_ARM
arm_goal = TOP_HOLDING_LEFT_ARM
#arm_goal = SIDE_HOLDING_LEFT_ARM
left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
set_joint_positions(pr2, left_joints, arm_start)
set_joint_positions(pr2, right_joints,
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, torso_joints, [0.2])
open_arm(pr2, 'left')
# test_ikfast(pr2)
p.addUserDebugLine(base_start, base_goal,
lineColorRGB=(1, 0, 0)) # addUserDebugText
print(base_start, base_goal)
if use_pr2_drake:
test_drake_base_motion(pr2, base_start, base_goal)
else:
test_base_motion(pr2, base_start, base_goal)
test_arm_motion(pr2, left_joints, arm_goal)
# test_arm_control(pr2, left_joints, arm_start)
user_input('Finish?')
disconnect()
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 LockRenderer():
with HideOutput():
robot = load_model(MOVO_URDF, fixed_base=True)
for link in get_links(robot):
set_color(robot,
color=apply_alpha(0.25 * np.ones(3), 1),
link=link)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
dump_body(robot)
wait_for_user('Start?')
#for arm in ARMS:
# test_close_gripper(robot, arm)
arm = 'right'
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
print(conf)
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_for_user()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_for_user()
test_retraction(robot,
MOVO_INFOS[arm],
tool_link,
fixed_joints=fixed_joints,
max_time=0.1)
disconnect()
def test_ikfast(pr2):
from pybullet_tools.ikfast.pr2.ik import get_tool_pose, get_ik_generator
left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
#right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
torso_left = torso_joints + left_joints
print(get_link_pose(pr2, link_from_name(pr2, 'l_gripper_tool_frame')))
# print(forward_kinematics('left', get_joint_positions(pr2, torso_left)))
print(get_tool_pose(pr2, 'left'))
arm = 'left'
pose = get_tool_pose(pr2, arm)
generator = get_ik_generator(pr2, arm, pose, torso_limits=False)
for i in range(100):
solutions = next(generator)
print(i, len(solutions))
for q in solutions:
set_joint_positions(pr2, torso_left, q)
wait_for_user()
def command_gripper(self,
arm,
position,
max_effort=NULL_EFFORT,
timeout=2.0,
blocking=True):
# position is the width of the gripper as in the physical distance between the two fingers
with ClientSaver(self.client):
gripper_joints = joints_from_names(
self.robot, self.get_gripper_joint_names(arm))
positions = [position] * len(gripper_joints)
self.follow_trajectory(gripper_joints, [positions],
max_sim_duration=timeout)
def command_arm_trajectory(self,
arm,
path,
times_from_start,
blocking=True,
**kwargs):
# angles is a list of joint angles, times is a list of times from start
# When calling joints on an arm, needs to be called with all the angles in the arm
# times is not used because our pybullet's position controller doesn't take into account times
assert len(path) == len(times_from_start)
with ClientSaver(self.client):
arm_joints = joints_from_names(self.robot,
self.get_arm_joint_names(arm))
self.follow_trajectory(arm_joints, path, times_from_start,
**kwargs)
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5):
floor_extent = 5.0
base_limits = (-floor_extent/2.*np.ones(2),
+floor_extent/2.*np.ones(2))
floor_height = 0.001
floor = create_box(floor_extent, floor_extent, floor_height, color=TAN)
set_point(floor, Point(z=-floor_height/2.))
wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY)
set_point(wall1, Point(y=floor_extent/2., z=wall_side/2.))
wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY)
set_point(wall2, Point(y=-floor_extent/2., z=wall_side/2.))
wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY)
set_point(wall3, Point(x=floor_extent/2., z=wall_side/2.))
wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY)
set_point(wall4, Point(x=-floor_extent/2., z=wall_side/2.))
walls = [wall1, wall2, wall3, wall4]
initial_surfaces = OrderedDict()
for _ in range(n_obstacles):
body = create_box(obst_width, obst_width, obst_height, color=GREY)
initial_surfaces[body] = floor
obstacles = walls + list(initial_surfaces)
initial_conf = np.array([+floor_extent/3, -floor_extent/3, 3*PI/4])
goal_conf = -initial_conf
with HideOutput():
robot = load_model(TURTLEBOT_URDF)
base_joints = joints_from_names(robot, BASE_JOINTS)
# base_link = child_link_from_joint(base_joints[-1])
base_link = link_from_name(robot, BASE_LINK_NAME)
set_all_color(robot, BLUE)
dump_body(robot)
set_point(robot, Point(z=stable_z(robot, floor)))
draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5)
set_joint_positions(robot, base_joints, initial_conf)
sample_placements(initial_surfaces, obstacles=[robot] + walls,
savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)],
min_distances=10e-2)
return robot, base_limits, goal_conf, obstacles
def solve_trac_ik(self, world_from_tool, nearby_tolerance=INF):
assert self.ik_solver is not None
init_lower, init_upper = self.ik_solver.get_joint_limits()
base_link = link_from_name(self.robot, self.ik_solver.base_link)
world_from_base = get_link_pose(self.robot, base_link)
tip_link = link_from_name(self.robot, self.ik_solver.tip_link)
tool_from_tip = multiply(
invert(get_link_pose(self.robot, self.tool_link)),
get_link_pose(self.robot, tip_link))
world_from_tip = multiply(world_from_tool, tool_from_tip)
base_from_tip = multiply(invert(world_from_base), world_from_tip)
joints = joints_from_names(
self.robot,
self.ik_solver.joint_names) # self.ik_solver.link_names
seed_state = get_joint_positions(self.robot, joints)
# seed_state = [0.0] * self.ik_solver.number_of_joints
lower, upper = init_lower, init_upper
if nearby_tolerance < INF:
tolerance = nearby_tolerance * np.ones(len(joints))
lower = np.maximum(lower, seed_state - tolerance)
upper = np.minimum(upper, seed_state + tolerance)
self.ik_solver.set_joint_limits(lower, upper)
(x, y, z), (rx, ry, rz, rw) = base_from_tip
# TODO: can also adjust tolerances
conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw)
self.ik_solver.set_joint_limits(init_lower, init_upper)
if conf is None:
return conf
# if nearby_tolerance < INF:
# print(lower.round(3))
# print(upper.round(3))
# print(conf)
# print(get_difference(seed_state, conf).round(3))
set_joint_positions(self.robot, joints, conf)
return get_configuration(self.robot)
def plan(robot, block, fixed, teleport):
grasp_gen = get_grasp_gen(robot, 'bottom', get_tool_frame(ARM)) #'top'
ik_fn = get_ik_fn(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION)
free_motion_fn = get_free_motion_gen(robot, fixed=([block] + fixed), teleport=teleport,
self_collisions=ENABLE_SELF_COLLISION)
holding_motion_fn = get_holding_motion_gen(robot, fixed=fixed, teleport=teleport,
self_collisions=ENABLE_SELF_COLLISION)
#arm_joints = get_torso_arm_joints(robot, ARM)
arm_joints = joints_from_names(robot, get_arm_joint_names(ARM))
pose0 = BodyPose(block)
conf0 = BodyConf(robot, joints=arm_joints)
saved_world = WorldSaver()
for grasp, in grasp_gen(block):
saved_world.restore()
result1 = ik_fn(block, pose0, grasp)
if result1 is None:
print('ik fn fails!')
continue
conf1, path2 = result1
pose0.assign()
result2 = free_motion_fn(conf0, conf1)
if result2 is None:
print("free motion plan fails!")
continue
path1, = result2
result3 = holding_motion_fn(conf1, conf0, block, grasp)
if result3 is None:
print('holding motion fails!')
continue
path3, = result3
return Command(path1.body_paths +
path2.body_paths +
path3.body_paths)
return None
def main():
# TODO: update this example
connect(use_gui=True)
with HideOutput():
pr2 = load_model(DRAKE_PR2_URDF)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']),
REST_LEFT_ARM)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']),
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']),
[0.2])
dump_body(pr2)
block = load_model(BLOCK_URDF, fixed_base=False)
set_point(block, [2, 0.5, 1])
target_point = point_from_pose(get_pose(block))
draw_point(target_point)
head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
#head_link = child_link_from_joint(head_joints[-1])
#head_name = get_link_name(pr2, head_link)
head_name = 'high_def_optical_frame' # HEAD_LINK_NAME | high_def_optical_frame | high_def_frame
head_link = link_from_name(pr2, head_name)
#max_detect_distance = 2.5
max_register_distance = 1.0
distance_range = (max_register_distance / 2, max_register_distance)
base_generator = visible_base_generator(pr2, target_point, distance_range)
base_joints = joints_from_names(pr2, PR2_GROUPS['base'])
for i in range(5):
base_conf = next(base_generator)
set_joint_positions(pr2, base_joints, base_conf)
handles = [
add_line(point_from_pose(get_link_pose(pr2, head_link)),
target_point,
color=RED),
add_line(point_from_pose(
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
target_point,
color=BLUE),
]
# head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
head_conf = inverse_visibility(pr2,
target_point,
head_name=head_name,
head_joints=head_joints)
assert head_conf is not None
set_joint_positions(pr2, head_joints, head_conf)
print(get_detections(pr2))
# TODO: does this detect the robot sometimes?
detect_mesh, z = get_detection_cone(pr2, block)
detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5))
set_pose(detect_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25))
set_pose(view_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
wait_if_gui()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()
def get_track_joint(robot):
return joints_from_names(robot, TRACK_JOINT)
def get_track_arm_joints(robot):
return joints_from_names(robot, TRACK_ARM_JOINT)
def base_joints(self):
return joints_from_names(self.robot, BASE_JOINTS)
def arm_joints(self):
#if self.robot_name == EVE:
# return get_eve_arm_joints(self.robot, arm=DEFAULT_ARM)
joint_names = ['panda_joint{}'.format(1 + i) for i in range(7)]
#joint_names = self.robot_yaml['cspace']
return joints_from_names(self.robot, joint_names)
def get_joint_positions(self, joint_names):
# Returns the configuration of the specified joints
with ClientSaver(self.client):
joints = joints_from_names(self.robot, joint_names)
return get_joint_positions(self.robot, joints)
def set_joint_positions(self, joint_names, positions):
with ClientSaver(self.client):
joints = joints_from_names(self.robot, joint_names)
set_joint_positions(self.robot, joints, positions)
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5):
floor_extent = 5.0
base_limits = (-floor_extent / 2. * np.ones(2),
+floor_extent / 2. * np.ones(2))
floor_height = 0.001
floor = create_box(floor_extent, floor_extent, floor_height, color=TAN)
set_point(floor, Point(z=-floor_height / 2.))
wall1 = create_box(floor_extent + wall_side,
wall_side,
wall_side,
color=GREY)
set_point(wall1, Point(y=floor_extent / 2., z=wall_side / 2.))
wall2 = create_box(floor_extent + wall_side,
wall_side,
wall_side,
color=GREY)
set_point(wall2, Point(y=-floor_extent / 2., z=wall_side / 2.))
wall3 = create_box(wall_side,
floor_extent + wall_side,
wall_side,
color=GREY)
set_point(wall3, Point(x=floor_extent / 2., z=wall_side / 2.))
wall4 = create_box(wall_side,
floor_extent + wall_side,
wall_side,
color=GREY)
set_point(wall4, Point(x=-floor_extent / 2., z=wall_side / 2.))
wall5 = create_box(obst_width, obst_width, obst_height, color=GREY)
set_point(wall5, Point(z=obst_height / 2.))
walls = [wall1, wall2, wall3, wall4, wall5]
initial_surfaces = OrderedDict()
for _ in range(n_obstacles - 1):
body = create_box(obst_width, obst_width, obst_height, color=GREY)
initial_surfaces[body] = floor
pillars = list(initial_surfaces)
obstacles = walls + pillars
initial_conf = np.array([+floor_extent / 3, -floor_extent / 3, 3 * PI / 4])
goal_conf = -initial_conf
robot = load_pybullet(TURTLEBOT_URDF,
fixed_base=True,
merge=True,
sat=False)
base_joints = joints_from_names(robot, BASE_JOINTS)
# base_link = child_link_from_joint(base_joints[-1])
base_link = link_from_name(robot, BASE_LINK_NAME)
set_all_color(robot, BLUE)
dump_body(robot)
set_point(robot, Point(z=stable_z(robot, floor)))
#set_point(robot, Point(z=base_aligned_z(robot)))
draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5)
set_joint_positions(robot, base_joints, initial_conf)
sample_placements(
initial_surfaces,
obstacles=[robot] + walls,
savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)],
min_distances=10e-2)
# The first calls appear to be the slowest
# times = []
# for body1, body2 in combinations(pillars, r=2):
# start_time = time.time()
# colliding = pairwise_collision(body1, body2)
# runtime = elapsed_time(start_time)
# print(colliding, runtime)
# times.append(runtime)
# print(times)
return robot, base_limits, goal_conf, obstacles
def get_arm_joints(robot, arm):
assert arm in ARMS
return joints_from_names(robot, names_from_templates(ARM_JOINTS, arm))
def main():
np.set_printoptions(precision=N_DIGITS, suppress=True,
threshold=3) # , edgeitems=1) #, linewidth=1000)
parser = argparse.ArgumentParser()
parser.add_argument(
'-a',
'--algorithm',
default='rrt_connect', # choices=[],
help='The motion planning algorithm to use.')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument(
'--holonomic',
action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false', help='')
parser.add_argument('-r', '--reversible', action='store_true', help='')
parser.add_argument(
'-s',
'--seed',
default=None,
type=int, # None | 1
help='The random seed to use.')
parser.add_argument('-n',
'--num',
default=10,
type=int,
help='The number of obstacles.')
parser.add_argument('-o', '--orientation', action='store_true', help='')
parser.add_argument('-v', '--viewer', action='store_false', help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
#set_aabb_buffer(buffer=1e-3)
#set_separating_axis_collisions()
#seed = 0
#seed = time.time()
seed = args.seed
if seed is None:
seed = random.randint(0, 10**3 - 1)
print('Seed:', seed)
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
#print('Random seed:', get_random_seed(), random.random())
#print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1(n_obstacles=args.num)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits(base_limits)
# draw_pose(get_link_pose(robot, base_link), length=0.5)
start_conf = get_joint_positions(robot, base_joints)
for conf in [start_conf, goal_conf]:
draw_waypoint(conf)
#resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
plan_joints = base_joints[:2] if not args.orientation else base_joints
d = len(plan_joints)
holonomic = args.holonomic or (d != 3)
resolutions = 1. * DEFAULT_RESOLUTION * np.ones(
d) # TODO: default resolutions, velocities, accelerations fns
#weights = np.reciprocal(resolutions)
weights = np.array([1, 1, 1e-3])[:d]
cost_fn = get_acceleration_fn(robot,
plan_joints,
max_velocities=MAX_VELOCITIES[:d],
max_accelerations=MAX_ACCELERATIONS[:d])
# TODO: check that taking shortest turning direction (reversible affecting?)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
#set_all_static() # Doesn't seem to affect
#test_aabb(robot)
#test_caching(robot, obstacles)
#return
#########################
# TODO: filter if straight-line path is feasible
saver = WorldSaver()
wait_for_duration(duration=1e-2)
start_time = time.time()
with LockRenderer(lock=args.lock):
with Profiler(field='cumtime', num=25): # tottime | cumtime | None
# TODO: draw the search tree
path = plan_base_joint_motion(
robot,
plan_joints,
goal_conf[:d],
holonomic=holonomic,
reversible=args.reversible,
obstacles=obstacles,
self_collisions=False,
custom_limits=custom_limits,
use_aabb=True,
cache=True,
max_distance=MIN_PROXIMITY,
resolutions=resolutions,
weights=weights, # TODO: use KDTrees
circular={
2: UNBOUNDED_LIMITS if holonomic else CIRCULAR_LIMITS
},
cost_fn=cost_fn,
algorithm=args.algorithm,
verbose=True,
restarts=5,
max_iterations=50,
smooth=None if holonomic else 100,
smooth_time=1, # None | 100 | INF
)
saver.restore()
# TODO: draw ROADMAPS
#wait_for_duration(duration=1e-3)
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = compute_cost(robot,
base_joints,
path,
resolutions=resolutions[:len(plan_joints)])
print(
'Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
wait_if_gui()
disconnect()
return
# for i, conf in enumerate(path):
# set_joint_positions(robot, plan_joints, conf)
# wait_if_gui('{}/{}) Continue?'.format(i + 1, len(path)))
path = extract_full_path(robot, plan_joints, path, base_joints)
with LockRenderer():
draw_last_roadmap(robot, base_joints)
# for i, conf in enumerate(path):
# set_joint_positions(robot, base_joints, conf)
# wait_if_gui('{}/{}) Continue?'.format(i+1, len(path)))
iterate_path(
robot,
base_joints,
path,
step_size=2e-2,
smooth=holonomic,
custom_limits=custom_limits,
resolutions=DEFAULT_RESOLUTION * np.ones(3), # resolutions
obstacles=obstacles,
self_collisions=False,
max_distance=MIN_PROXIMITY)
disconnect()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument('--holonomic', action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false',
help='')
parser.add_argument('-s', '--seed', default=None, type=int,
help='The random seed to use.')
parser.add_argument('-v', '--viewer', action='store_false',
help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
seed = args.seed
#seed = 0
#seed = time.time()
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
print('Random seed:', get_random_seed(), random.random())
print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1()
draw_base_limits(base_limits)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
base_link = link_from_name(robot, BASE_LINK_NAME)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
set_all_static() # Doesn't seem to affect
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
step_simulation() # Need to call before get_bodies_in_region
#update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331
bodies = get_bodies_in_region(region_aabb)
print(len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
#draw_pose(get_link_pose(robot, base_link), length=0.5)
for conf in [get_joint_positions(robot, base_joints), goal_conf]:
draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH)
aabb = get_aabb(robot)
#aabb = get_subtree_aabb(robot, base_link)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
#########################
saver = WorldSaver()
start_time = time.time()
profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None
profiler.save()
with LockRenderer(lock=args.lock):
path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles,
custom_limits=custom_limits, resolutions=resolutions,
use_aabb=True, cache=True, max_distance=0.,
restarts=2, iterations=20, smooth=20) # 20 | None
saver.restore()
#wait_for_duration(duration=1e-3)
profiler.restore()
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path))
print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
disconnect()
return
iterate_path(robot, base_joints, path)
disconnect()
def close_gripper_test(problem):
joints = joints_from_names(problem.robot, PR2_GROUPS['left_gripper'])
values = [get_min_limit(problem.robot, joint) for joint in joints]
for _ in joint_controller_hold(problem.robot, joints, values):
enable_gravity()
step_simulation()
def main(num_iterations=10):
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
side = 0.05
block = create_box(w=side, l=side, h=side, color=RED)
start_time = time.time()
with LockRenderer():
with HideOutput():
# TODO: MOVO must be loaded last
robot = load_model(MOVO_URDF, fixed_base=True)
#set_all_color(robot, color=MOVO_COLOR)
assign_link_colors(robot)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
print('Load time: {:.3f}'.format(elapsed_time(start_time)))
dump_body(robot)
#print(get_colliding(robot))
#for arm in ARMS:
# test_close_gripper(robot, arm)
#test_grasps(robot, block)
arm = RIGHT
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_info = MOVO_INFOS[arm]
print_ik_warning(ik_info)
ik_joints = get_ik_joints(robot, ik_info, tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
wait_if_gui('Start?')
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(num_iterations):
conf = sample_fn()
print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations,
np.array(conf)))
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_if_gui()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_if_gui()
test_retraction(robot,
ik_info,
tool_link,
fixed_joints=fixed_joints,
max_time=0.05,
max_candidates=100)
disconnect()
def get_ik_fn(robot, fixed=[], teleport=False, num_attempts=10, self_collisions=True):
# movable_joints = get_movable_joints(robot)
torso_arm = get_torso_arm_joints(robot, ARM)
arm_joints = joints_from_names(robot, get_arm_joint_names(ARM))
sample_fn = get_sample_fn(robot, torso_arm)
def fn(body, pose, grasp):
obstacles = [body] + fixed
gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose)
approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose)
draw_pose(get_tool_pose(robot, ARM), length=0.04)
draw_pose(approach_pose, length=0.04)
draw_pose(gripper_pose, length=0.04)
# print(movable_joints)
# print(torso_arm)
# wait_for_interrupt()
# TODO: gantry_x_joint
# TODO: proper inverse reachability
base_link = link_from_name(robot, IK_BASE_FRAMES[ARM])
base_pose = get_link_pose(robot, base_link)
body_point_in_base = point_from_pose(multiply(invert(base_pose), pose.pose))
y_joint = joint_from_name(robot, '{}_gantry_y_joint'.format(prefix_from_arm(ARM)))
initial_y = get_joint_position(robot, y_joint)
ik_y = initial_y + SIGN_FROM_ARM[ARM]*body_point_in_base[1]
set_joint_position(robot, y_joint, ik_y)
for _ in range(num_attempts):
if USE_IKFAST:
q_approach = sample_tool_ik(robot, ARM, approach_pose)
if q_approach is not None:
set_joint_positions(robot, torso_arm, q_approach)
else:
set_joint_positions(robot, torso_arm, sample_fn()) # Random seed
q_approach = inverse_kinematics(robot, grasp.link, approach_pose)
if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles):
print('- ik for approaching fails!')
continue
conf = BodyConf(robot, joints=arm_joints)
if USE_IKFAST:
q_grasp = sample_tool_ik(robot, ARM, gripper_pose, closest_only=True)
if q_grasp is not None:
set_joint_positions(robot, torso_arm, q_grasp)
else:
conf.assign()
q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose)
if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles):
print('- ik for grasp fails!')
continue
if teleport:
path = [q_approach, q_grasp]
else:
conf.assign()
#direction, _ = grasp.approach_pose
#path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction,
# quat_from_pose(approach_pose))
path = plan_direct_joint_motion(robot, torso_arm, q_grasp, obstacles=obstacles,
self_collisions=self_collisions)
if path is None:
if DEBUG_FAILURE:
print('Approach motion failed')
continue
command = Command([BodyPath(robot, path, joints=torso_arm),
Attach(body, robot, grasp.link),
BodyPath(robot, path[::-1], joints=torso_arm, attachments=[grasp])])
return (conf, command)
# TODO: holding collisions
return None
return fn
