def solve_inverse_kinematics(self,
world_from_tool,
nearby_tolerance=INF,
**kwargs):
if self.ik_solver is not None:
return self.solve_trac_ik(world_from_tool, **kwargs)
#if nearby_tolerance != INF:
# return self.solve_pybullet_ik(world_from_tool, nearby_tolerance=nearby_tolerance)
current_conf = get_joint_positions(self.robot, self.arm_joints)
start_time = time.time()
if nearby_tolerance == INF:
generator = ikfast_inverse_kinematics(self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_attempts=10,
use_halton=True)
else:
generator = closest_inverse_kinematics(
self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_time=0.01,
max_distance=nearby_tolerance,
use_halton=True)
conf = next(generator, None)
#conf = sample_tool_ik(self.robot, world_from_tool, max_attempts=100)
if conf is None:
return conf
max_distance = get_distance(current_conf, conf, norm=INF)
#print('Time: {:.3f} | distance: {:.5f} | max: {:.5f}'.format(
# elapsed_time(start_time), max_distance, nearby_tolerance))
set_joint_positions(self.robot, self.arm_joints, conf)
return get_configuration(self.robot)
def sample_tool_ik(robot,
tool_pose,
max_attempts=10,
closest_only=False,
get_all=False,
prev_free_list=[],
**kwargs):
generator = get_ik_generator(robot,
tool_pose,
prev_free_list=prev_free_list,
**kwargs)
ik_joints = get_movable_joints(robot)
for _ in range(max_attempts):
try:
solutions = next(generator)
if closest_only and solutions:
current_conf = get_joint_positions(robot, ik_joints)
solutions = [
min(solutions,
key=lambda conf: get_distance(current_conf, conf))
]
solutions = list(
filter(
lambda conf: not violates_limits(robot, ik_joints, conf),
solutions))
return solutions if get_all else select_solution(
robot, ik_joints, solutions, **kwargs)
except StopIteration:
break
return None
def experimental_inverse_kinematics(robot,
link,
pose,
null_space=False,
max_iterations=200,
tolerance=1e-3):
(point, quat) = pose
# https://github.com/bulletphysics/bullet3/blob/389d7aaa798e5564028ce75091a3eac6a5f76ea8/examples/SharedMemory/PhysicsClientC_API.cpp
# https://github.com/bulletphysics/bullet3/blob/c1ba04a5809f7831fa2dee684d6747951a5da602/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
joints = get_joints(
robot) # Need to have all joints (although only movable returned)
movable_joints = get_movable_joints(robot)
current_conf = get_joint_positions(robot, joints)
# TODO: sample other values for the arm joints as the reference conf
min_limits = [get_joint_limits(robot, joint)[0] for joint in joints]
max_limits = [get_joint_limits(robot, joint)[1] for joint in joints]
#min_limits = current_conf
#max_limits = current_conf
#max_velocities = [get_max_velocity(robot, joint) for joint in joints] # Range of Jacobian
max_velocities = [10000] * len(joints)
# TODO: cannot have zero velocities
# TODO: larger definitely better for velocities
#damping = tuple(0.1*np.ones(len(joints)))
#t0 = time.time()
#kinematic_conf = get_joint_positions(robot, movable_joints)
for iterations in range(max_iterations): # 0.000863273143768 / iteration
# TODO: return none if no progress
if null_space:
kinematic_conf = p.calculateInverseKinematics(
robot,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf,
#jointDamping=damping,
)
else:
kinematic_conf = p.calculateInverseKinematics(
robot, link, point, quat)
if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
return None
set_joint_positions(robot, movable_joints, kinematic_conf)
link_point, link_quat = get_link_pose(robot, link)
if np.allclose(link_point, point, atol=tolerance) and np.allclose(
link_quat, quat, atol=tolerance):
#print iterations
break
else:
return None
if violates_limits(robot, movable_joints, kinematic_conf):
return None
#total_time = (time.time() - t0)
#print total_time
#print (time.time() - t0)/max_iterations
return kinematic_conf
def translate_linearly(world, distance):
# TODO: could just apply in the base frame
x, y, theta = get_joint_positions(world.robot, world.base_joints)
pos = np.array([x, y])
goal_pos = pos + distance * unit_from_theta(theta)
goal_pose = np.append(goal_pos, [theta])
return goal_pose
def extract_full_path(robot, path_joints, path, all_joints):
with BodySaver(robot):
new_path = []
for conf in path:
set_joint_positions(robot, path_joints, conf)
new_path.append(get_joint_positions(
robot, all_joints)) # TODO: do without assigning
return new_path
def sample_tool_ik(robot, tool_pose, closest_only=False, get_all=False, **kwargs):
generator = get_ik_generator(robot, tool_pose)
ik_joints = get_movable_joints(robot)
solutions = next(generator)
if closest_only and solutions:
current_conf = get_joint_positions(robot, ik_joints)
solutions = [min(solutions, key=lambda conf: get_distance(current_conf, conf))]
solutions = list(filter(lambda conf: not violates_limits(robot, ik_joints, conf), solutions))
return solutions if get_all else select_solution(robot, ik_joints, solutions, **kwargs)
def iterate(self, world, attachments):
joints = get_gripper_joints(world.robot, self.arm)
start_conf = get_joint_positions(world.robot, joints)
end_conf = [self.position] * len(joints)
extend_fn = get_extend_fn(world.robot, joints)
path = [start_conf] + list(extend_fn(start_conf, end_conf))
for positions in path:
set_joint_positions(world.robot, joints, positions)
yield positions
def get_tool_pose(robot):
from .ikfast_kuka_kr6_r900 import get_fk
ik_joints = get_movable_joints(robot)
conf = get_joint_positions(robot, ik_joints)
# TODO: this should be linked to ikfast's get numOfJoint function
assert len(conf) == 6
base_from_tool = compute_forward_kinematics(get_fk, conf)
world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME))
return multiply(world_from_base, base_from_tool)
def solve_pybullet_ik(self, world_from_tool, nearby_tolerance):
start_time = time.time()
# Most of the time is spent creating the robot
# TODO: use the waypoint version that doesn't repeatedly create the robot
current_conf = get_joint_positions(self.robot, self.arm_joints)
full_conf = sub_inverse_kinematics(
self.robot,
self.arm_joints[0],
self.tool_link,
world_from_tool,
custom_limits=self.custom_limits
) # , max_iterations=1) # , **kwargs)
conf = get_joint_positions(self.robot, self.arm_joints)
max_distance = get_distance(current_conf, conf, norm=INF)
if nearby_tolerance < max_distance:
return None
print('Nearby) time: {:.3f} | distance: {:.5f}'.format(
elapsed_time(start_time), max_distance))
return full_conf
def is_gripper_closed(self):
# TODO: base this on the object type
if self.holding is not None:
obj_type = type_from_name(self.holding)
if obj_type not in TIN_OBJECTS:
return False
# each joint in [0.00, 0.04] (units coincide with meters on the physical gripper)
current_gq = get_joint_positions(self.world.robot,
self.world.gripper_joints)
gripper_width = sum(current_gq)
return gripper_width <= MIN_GRASP_WIDTH
def get_tool_pose(robot, arm):
from .ikfast_eth_rfl import get_fk
ik_joints = get_torso_arm_joints(robot, arm)
conf = get_joint_positions(robot, ik_joints)
# TODO: this should be linked to ikfast's get numOfJoint junction
base_from_ik = compute_forward_kinematics(get_fk, conf)
base_from_tool = multiply(base_from_ik,
invert(get_tool_from_ik(robot, arm)))
world_from_base = get_link_pose(robot,
link_from_name(robot, IK_BASE_FRAMES[arm]))
return multiply(world_from_base, base_from_tool)
def follow_curve(robot,
joints,
curve,
goal_t=None,
time_step=None,
max_velocities=MAX_VELOCITIES,
**kwargs):
if goal_t is None:
goal_t = curve.x[-1]
if time_step is None:
time_step = 10 * get_time_step()
#distance_fn = get_distance_fn(robot, joints, weights=None, norm=2)
distance_fn = get_duration_fn(robot,
joints,
velocities=max_velocities,
norm=INF) # get_distance
positions = np.array(get_joint_positions(robot, joints))
closest_dist, closest_t = find_closest(positions,
curve,
t_range=(curve.x[0], goal_t),
max_time=1e-1,
max_iterations=INF,
distance_fn=distance_fn,
verbose=True)
print('Closest dist: {:.3f} | Closest time: {:.3f}'.format(
closest_dist, closest_t))
target_t = closest_t
# TODO: condition based on closest_dist
while True:
print('\nTarget time: {:.3f} | Goal time: {}'.format(target_t, goal_t))
target_positions = curve(target_t)
target_velocities = curve(target_t, nu=1) # TODO: draw the velocity
#print('Positions: {} | Velocities: {}'.format(target_positions, target_velocities))
handles = draw_waypoint(target_positions)
is_goal = (target_t == goal_t)
position_tol = 1e-2 if is_goal else 1e-2
for output in control_state(robot,
joints,
target_positions=target_positions,
target_velocities=target_velocities,
position_tol=position_tol,
velocity_tol=INF,
max_velocities=max_velocities,
**kwargs):
yield output
remove_handles(handles)
target_t = min(goal_t, target_t + time_step)
if is_goal:
break
def plan_water_motion(body,
joints,
end_conf,
attachment,
obstacles=None,
attachments=[],
self_collisions=True,
disabled_collisions=set(),
max_distance=MAX_DISTANCE,
weights=None,
resolutions=None,
reference_pose=unit_pose(),
custom_limits={},
**kwargs):
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(body, joints, weights=weights)
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(body,
joints,
obstacles, {attachment} | set(attachments),
self_collisions,
disabled_collisions,
max_distance=max_distance,
custom_limits=custom_limits)
def water_test(q):
if attachment is None:
return False
set_joint_positions(body, joints, q)
attachment.assign()
attachment_pose = get_pose(attachment.child)
pose = multiply(attachment_pose,
reference_pose) # TODO: confirm not inverted
roll, pitch, _ = euler_from_quat(quat_from_pose(pose))
violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch))
#if violation: # TODO: check whether different confs can be waypoints for each object
# print(q, violation)
# wait_for_user()
return violation
invalid_fn = lambda q, **kwargs: water_test(q) or collision_fn(q, **kwargs)
start_conf = get_joint_positions(body, joints)
if not check_initial_end(start_conf, end_conf, invalid_fn):
return None
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
invalid_fn, **kwargs)
def test_ikfast(pr2):
from pybullet_tools.pr2_ik.ik import forward_kinematics, inverse_kinematics, 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_interrupt()
def control_state(robot,
joints,
target_positions,
target_velocities=None,
position_tol=INF,
velocity_tol=INF,
max_velocities=None,
verbose=True): # TODO: max_accelerations
if target_velocities is None:
target_velocities = np.zeros(len(joints))
if max_velocities is None:
max_velocities = get_max_velocities(robot, joints)
assert (max_velocities > 0).all()
max_velocity_control_joints(robot,
joints,
positions=target_positions,
velocities=target_velocities,
max_velocities=max_velocities)
for i in irange(INF):
current_positions = np.array(get_joint_positions(robot, joints))
position_error = get_distance(current_positions,
target_positions,
norm=INF)
current_velocities = np.array(get_joint_velocities(robot, joints))
velocity_error = get_distance(current_velocities,
target_velocities,
norm=INF)
if verbose:
# print('Positions: {} | Target positions: {}'.format(current_positions.round(N_DIGITS), target_positions.round(N_DIGITS)))
# print('Velocities: {} | Target velocities: {}'.format(current_velocities.round(N_DIGITS), target_velocities.round(N_DIGITS)))
print(
'Step: {} | Position error: {:.3f}/{:.3f} | Velocity error: {:.3f}/{:.3f}'
.format(i, position_error, position_tol, velocity_error,
velocity_tol))
# TODO: draw the tolerance interval
if (position_error <= position_tol) and (velocity_error <=
velocity_tol):
return # TODO: declare success or failure by yielding or throwing an exception
yield i
def plan_workspace(world,
tool_path,
obstacles,
randomize=True,
teleport=False):
# Assuming that pairs of fixed things aren't in collision at this point
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
distance_fn = get_distance_fn(world.robot, world.arm_joints)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
arm_path = []
for i, tool_pose in enumerate(tool_path):
#set_joint_positions(world.kitchen, [door_joint], door_path[i])
tolerance = INF if i == 0 else NEARBY_PULL
full_arm_conf = world.solve_inverse_kinematics(
tool_pose, nearby_tolerance=tolerance)
if full_arm_conf is None:
# TODO: this fails when teleport=True
if PRINT_FAILURES: print('Workspace kinematic failure')
return None
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
if PRINT_FAILURES: print('Workspace collision failure')
return None
arm_conf = get_joint_positions(world.robot, world.arm_joints)
if arm_path and not teleport:
distance = distance_fn(arm_path[-1], arm_conf)
if MAX_CONF_DISTANCE < distance:
if PRINT_FAILURES:
print(
'Workspace proximity failure (distance={:.5f})'.format(
distance))
return None
arm_path.append(arm_conf)
# wait_for_user()
return arm_path
def draw_last_roadmap(robot,
joints,
only_checked=False,
linear=True,
down_sample=None,
**kwargs):
q0 = get_joint_positions(robot, joints)
handles = []
if not ROADMAPS:
return handles
roadmap = ROADMAPS[-1]
for q in roadmap.samples:
q = q if len(q) == 3 else np.append(q[:2],
q0[2:]) # TODO: make a function
handles.extend(draw_pose2d(q, z=DRAW_Z))
for v1, v2 in roadmap.edges:
color = BLACK
if roadmap.is_colliding(v1, v2):
color = RED
elif roadmap.is_safe(v1, v2):
color = GREEN
elif only_checked:
continue
if linear:
path = [roadmap.samples[v1], roadmap.samples[v2]]
else:
path = roadmap.get_path(v1, v2)
if down_sample is not None:
path = path[::down_sample] + [path[-1]]
#handles.extend(draw_path(path, **kwargs))
points = list(
map(point_from_pose, [
pose_from_pose2d(
q if len(q) == 3 else np.append(q[:2], q0[2:]), z=DRAW_Z)
for q in path
]))
handles.extend(
add_line(p1, p2, color=color) for p1, p2 in get_pairs(points))
return handles
def rest_for_duration(self, duration):
if not self.execute_motor_control:
return
sim_duration = 0.0
body = self.robot
position_gain = 0.02
with ClientSaver(self.client):
# TODO: apply to all joints
dt = get_time_step()
movable_joints = get_movable_joints(body)
target_conf = get_joint_positions(body, movable_joints)
while sim_duration < duration:
p.setJointMotorControlArray(
body,
movable_joints,
p.POSITION_CONTROL,
targetPositions=target_conf,
targetVelocities=[0.0] * len(movable_joints),
positionGains=[position_gain] * len(movable_joints),
# velocityGains=[velocity_gain] * len(movable_joints),
physicsClientId=self.client)
step_simulation()
sim_duration += dt
def optimize_angle(end_effector,
element_pose,
translation,
direction,
reverse,
candidate_angles,
collision_fn,
nearby=True,
max_error=TRANSLATION_TOLERANCE):
robot = end_effector.robot
movable_joints = get_movable_joints(robot)
best_error, best_angle, best_conf = max_error, None, None
initial_conf = get_joint_positions(robot, movable_joints)
for angle in candidate_angles:
grasp_pose = get_grasp_pose(translation, direction, angle, reverse)
# Pose_{world,EE} = Pose_{world,element} * Pose_{element,EE}
# = Pose_{world,element} * (Pose_{EE,element})^{-1}
target_pose = multiply(element_pose, invert(grasp_pose))
set_pose(end_effector.body,
multiply(target_pose, end_effector.tool_from_ee))
if nearby:
set_joint_positions(robot, movable_joints, initial_conf)
conf = solve_ik(end_effector, target_pose, nearby=nearby)
if (conf is None) or collision_fn(conf):
continue
set_joint_positions(robot, movable_joints, conf)
link_pose = get_link_pose(robot, end_effector.tool_link)
error = get_distance(point_from_pose(target_pose),
point_from_pose(link_pose))
if error < best_error: # TODO: error a function of direction as well
best_error, best_angle, best_conf = error, angle, conf
#break
if best_conf is not None:
set_joint_positions(robot, movable_joints, best_conf)
return best_angle, best_conf
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 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 solve_collision_free(door,
obstacle,
max_iterations=100,
step_size=math.radians(5),
min_distance=2e-2,
draw=True):
joints = get_movable_joints(door)
door_link = child_link_from_joint(joints[-1])
# print(get_com_pose(door, door_link))
# print(get_link_inertial_pose(door, door_link))
# print(get_link_pose(door, door_link))
# draw_pose(get_com_pose(door, door_link))
handles = []
success = False
start_time = time.time()
for iteration in range(max_iterations):
current_conf = np.array(get_joint_positions(door, joints))
collision_infos = get_closest_points(door,
obstacle,
link1=door_link,
max_distance=min_distance)
if not collision_infos:
success = True
break
collision_infos = sorted(collision_infos,
key=lambda info: info.contactDistance)
collision_infos = collision_infos[:1] # TODO: average all these
if draw:
for collision_info in collision_infos:
handles.extend(draw_collision_info(collision_info))
wait_if_gui()
[collision_info] = collision_infos[:1]
distance = collision_info.contactDistance
print(
'Iteration: {} | Collisions: {} | Distance: {:.3f} | Time: {:.3f}'.
format(iteration, len(collision_infos), distance,
elapsed_time(start_time)))
if distance >= min_distance:
success = True
break
# TODO: convergence or decay in step size
direction = step_size * get_unit_vector(
collision_info.contactNormalOnB) # B->A (already normalized)
contact_point = collision_info.positionOnA
#com_pose = get_com_pose(door, door_link) # TODO: be careful here
com_pose = get_link_pose(door, door_link)
local_point = tform_point(invert(com_pose), contact_point)
#local_point = unit_point()
translate, rotate = compute_jacobian(door,
door_link,
point=local_point)
delta_conf = np.array([
np.dot(translate[mj], direction) # + np.dot(rotate[mj], direction)
for mj in movable_from_joints(door, joints)
])
new_conf = current_conf + delta_conf
if violates_limits(door, joints, new_conf):
break
set_joint_positions(door, joints, new_conf)
if draw:
wait_if_gui()
remove_handles(handles)
print('Success: {} | Iteration: {} | Time: {:.3f}'.format(
success, iteration, elapsed_time(start_time)))
#quit()
return success
def follow_curve_old(robot, joints, curve, goal_t=None):
# TODO: unify with /Users/caelan/Programs/open-world-tamp/open_world/simulation/control.py
if goal_t is None:
goal_t = curve.x[-1]
time_step = get_time_step()
target_step = 10 * time_step
#distance_fn = get_distance_fn(robot, joints, weights=None, norm=2)
distance_fn = get_duration_fn(robot,
joints,
velocities=MAX_VELOCITIES,
norm=INF)
for i in irange(INF):
# if (i % 10) != 0:
# continue
current_p = np.array(get_joint_positions(robot, joints))
current_v = np.array(get_joint_velocities(robot, joints))
goal_dist = distance_fn(current_p, curve(goal_t))
print('Positions: {} | Velocities: {} | Goal distance: {:.3f}'.format(
current_p.round(N_DIGITS), current_v.round(N_DIGITS), goal_dist))
if goal_dist < 1e-2:
return True
# _, connection = mpc(current_p, current_v, curve, v_max=MAX_VELOCITIES, a_max=MAX_ACCELERATIONS,
# dt_max=1e-1, max_time=1e-1)
# assert connection is not None
# target_t = 0.5*connection.x[-1]
# target_p = connection(target_t)
# target_v = connection(target_t, nu=1)
# #print(target_p)
closest_dist, closest_t = find_closest(current_p,
curve,
t_range=None,
max_time=1e-2,
max_iterations=INF,
distance_fn=distance_fn,
verbose=True)
target_t = min(closest_t + target_step, curve.x[-1])
target_p = curve(target_t)
#target_v = curve(target_t, nu=1)
target_v = curve(closest_t, nu=1)
#target_v = MAX_VELOCITIES
#target_v = INF*np.zeros(len(joints))
handles = draw_waypoint(target_p)
#times, confs = time_discretize_curve(curve, verbose=False, resolution=resolutions) # max_velocities=v_max,
# set_joint_positions(robot, joints, target_p)
max_velocity_control_joints(robot,
joints,
positions=target_p,
velocities=target_v,
max_velocities=MAX_VELOCITIES)
#next_t = closest_t + time_step
#next_p = current_p + current_v*time_step
yield target_t
actual_p = np.array(get_joint_positions(robot, joints))
actual_v = np.array(get_joint_velocities(robot, joints))
next_p = current_p + actual_v * time_step
print('Predicted: {} | Actual: {}'.format(next_p.round(N_DIGITS),
actual_p.round(N_DIGITS)))
remove_handles(handles)
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 get_base_conf(self):
return get_joint_positions(self.robot, self.base_joints)
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_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 follow_trajectory(self,
joints,
path,
times_from_start=None,
real_time_step=0.0,
waypoint_tolerance=1e-2 * np.pi,
goal_tolerance=5e-3 * np.pi,
max_sim_duration=1.0,
print_sim_frequency=1.0,
**kwargs):
# real_time_step = 1e-1 # Can optionally sleep to slow down visualization
start_time = time.time()
if times_from_start is not None:
assert len(path) == len(times_from_start)
current_positions = get_joint_positions(self.robot, joints)
differences = [(np.array(q2) - np.array(q1)) / (t2 - t1)
for q1, t1, q2, t2 in
zip([current_positions] + path, [0.] +
times_from_start, path, times_from_start)]
velocity_path = differences[1:] + [np.zeros(len(joints))
] # Using velocity at endpoints
else:
velocity_path = [None] * len(path)
sim_duration = 0.0
sim_steps = 0
last_print_sim_duration = sim_duration
with ClientSaver(self.client):
dt = get_time_step()
# TODO: fit using splines to get velocity info
for num, positions in enumerate(path):
if self.execute_motor_control:
sim_start = sim_duration
tolerance = goal_tolerance if (num == len(path) -
1) else waypoint_tolerance
velocities = velocity_path[num]
for _ in joint_controller_hold2(self.robot,
joints,
positions,
velocities,
tolerance=tolerance,
**kwargs):
step_simulation()
# print(get_joint_velocities(self.robot, joints))
# print([get_joint_torque(self.robot, joint) for joint in joints])
sim_duration += dt
sim_steps += 1
time.sleep(real_time_step)
if print_sim_frequency <= (sim_duration -
last_print_sim_duration):
print(
'Waypoint: {} | Simulation steps: {} | Simulation seconds: {:.3f} | Steps/sec {:.3f}'
.format(num, sim_steps, sim_duration,
sim_steps / elapsed_time(start_time)))
last_print_sim_duration = sim_duration
if max_sim_duration <= (sim_duration - sim_start):
print(
'Timeout of {:.3f} simulation seconds exceeded!'
.format(max_sim_duration))
break
# control_joints(self.robot, arm_joints, positions)
else:
set_joint_positions(self.robot, joints, positions)
print(
'Followed {} waypoints in {:.3f} simulation seconds and {} simulation steps'
.format(len(path), sim_duration, sim_steps))
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 main(floor_width=2.0):
# Creates a pybullet world and a visualizer for it
connect(use_gui=True)
identity_pose = (unit_point(), unit_quat())
origin_handles = draw_pose(
identity_pose, length=1.0
) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE)
# Bodies are described by an integer index
floor = create_box(w=floor_width, l=floor_width, h=0.001,
color=TAN) # Creates a tan box object for the floor
set_point(floor,
[0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor
obstacle = create_box(w=0.5, l=0.5, h=0.1,
color=RED) # Creates a red box obstacle
set_point(
obstacle,
[0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle
print('Position:', get_point(obstacle))
set_euler(obstacle,
[0, 0, np.pi / 4
]) # Sets the [roll,pitch,yaw] orientation of the obstacle
print('Orientation:', get_euler(obstacle))
with LockRenderer(
): # Temporarily prevents the renderer from updating for improved loading efficiency
with HideOutput(): # Temporarily suppresses pybullet output
robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file
robot_z = stable_z(
robot, floor
) # Returns the z offset required for robot to be placed on floor
set_point(robot,
[0, 0, robot_z]) # Sets the z position of the robot
dump_body(robot) # Prints joint and link information about robot
set_all_static()
# Joints are also described by an integer index
# The turtlebot has explicit joints representing x, y, theta
x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x'
y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y'
theta_joint = joint_from_name(
robot, 'theta') # Looks up the robot joint named 'theta'
joints = [x_joint, y_joint, theta_joint]
base_link = link_from_name(
robot, 'base_link') # Looks up the robot link named 'base_link'
world_from_obstacle = get_pose(
obstacle
) # Returns the pose of the origin of obstacle wrt the world frame
obstacle_aabb = get_subtree_aabb(obstacle)
draw_aabb(obstacle_aabb)
random.seed(0) # Sets the random number generator state
handles = []
for i in range(10):
for handle in handles:
remove_debug(handle)
print('\nIteration: {}'.format(i))
x = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, x_joint,
x) # Sets the current value of the x joint
y = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, y_joint,
y) # Sets the current value of the y joint
yaw = random.uniform(-np.pi, np.pi)
set_joint_position(robot, theta_joint,
yaw) # Sets the current value of the theta joint
values = get_joint_positions(
robot,
joints) # Obtains the current values for the specified joints
print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values))
world_from_robot = get_link_pose(
robot,
base_link) # Returns the pose of base_link wrt the world frame
position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion)
x, y, z = position # Decomposing position into x, y, z
print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format(
x, y, z))
euler = euler_from_quat(
quaternion) # Converting from quaternion to euler angles
roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw
print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'.
format(roll, pitch, yaw))
handles.extend(
draw_pose(world_from_robot, length=0.5)
) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE)
obstacle_from_robot = multiply(
invert(world_from_obstacle),
world_from_robot) # Relative transformation from robot to obstacle
robot_aabb = get_subtree_aabb(
robot,
base_link) # Computes the robot's axis-aligned bounding box (AABB)
lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema
center = (lower + upper) / 2. # Computing the center of the AABB
extent = upper - lower # Computing the dimensions of the AABB
handles.extend(draw_aabb(robot_aabb))
collision = pairwise_collision(
robot, obstacle
) # Checks whether robot is currently colliding with obstacle
print('Collision: {}'.format(collision))
wait_for_duration(1.0) # Like sleep() but also updates the viewer
wait_for_user() # Like raw_input() but also updates the viewer
# Destroys the pybullet world
disconnect()
