def go_to_conf(self, conf):
control_joints(
self.pw.robot,
get_movable_joints(self.pw.robot) + list(self.finger_joints),
tuple(conf) + (self.target_grip, self.target_grip))
simulate_for_duration(0.3)
self.steps_taken += 0.3
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 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 get_motion_fn(robot, brick_from_index, obstacle_from_name, teleport=False):
movable_joints = get_movable_joints(robot)
disabled_collisions = get_disabled_collisions(robot)
def fn(conf1, conf2, fluents=[]):
if teleport is True:
path = [conf1, conf2]
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
# TODO: double check that collisions with movable obstacles are considered
obstacles = list(obstacle_from_name.values())
attachments = parse_fluents(robot, brick_from_index, fluents,
obstacles)
set_joint_positions(robot, movable_joints, conf1)
path = plan_joint_motion(
robot,
movable_joints,
conf2,
obstacles=obstacles,
attachments=attachments,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
#weights=weights, resolutions=resolutions,
restarts=5,
iterations=50,
smooth=100)
if path is None:
return None
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
return fn
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_user()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_user()
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 plan(robot, block, fixed, teleport):
grasp_gen = get_grasp_gen(robot, 'top', TOOL_FRAME)
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)
movable_joints = get_track_arm_joints(robot) if USE_IKFAST else get_movable_joints(robot)
pose0 = BodyPose(block)
conf0 = BodyConf(robot, joints=movable_joints)
saved_world = WorldSaver()
for grasp, in grasp_gen(block):
saved_world.restore()
result1 = ik_fn(block, pose0, grasp)
if result1 is None:
continue
conf1, path2 = result1
pose0.assign()
result2 = free_motion_fn(conf0, conf1)
if result2 is None:
continue
path1, = result2
result3 = holding_motion_fn(conf1, conf0, block, grasp)
if result3 is None:
continue
path3, = result3
return Command(path1.body_paths + path2.body_paths + path3.body_paths)
return None
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 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 __init__(self, robot, positions=None, node=None, element=None):
self.robot = robot
self.joints = get_movable_joints(self.robot)
if positions is None:
positions = get_configuration(self.robot)
self.positions = positions
self.node = node
self.element = element
def main(group=SE3):
connect(use_gui=True)
set_camera_pose(camera_point=SIZE*np.array([1., -1., 1.]))
# TODO: can also create all links and fix some joints
# TODO: SE(3) motion planner (like my SE(3) one) where some dimensions are fixed
obstacle = create_box(w=SIZE, l=SIZE, h=SIZE, color=RED)
#robot = create_cylinder(radius=0.025, height=0.1, color=BLUE)
obstacles = [obstacle]
collision_id, visual_id = create_shape(get_cylinder_geometry(radius=0.025, height=0.1), color=BLUE)
robot = create_flying_body(group, collision_id, visual_id)
body_link = get_links(robot)[-1]
joints = get_movable_joints(robot)
joint_from_group = dict(zip(group, joints))
print(joint_from_group)
#print(get_aabb(robot, body_link))
dump_body(robot, fixed=False)
custom_limits = {joint_from_group[j]: l for j, l in CUSTOM_LIMITS.items()}
# sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
# for i in range(10):
# conf = sample_fn()
# set_joint_positions(robot, joints, conf)
# wait_for_user('Iteration: {}'.format(i))
# return
initial_point = SIZE*np.array([-1., -1., 0])
#initial_point = -1.*np.ones(3)
final_point = -initial_point
initial_euler = np.zeros(3)
add_line(initial_point, final_point, color=GREEN)
initial_conf = np.concatenate([initial_point, initial_euler])
final_conf = np.concatenate([final_point, initial_euler])
set_joint_positions(robot, joints, initial_conf)
#print(initial_point, get_link_pose(robot, body_link))
#set_pose(robot, Pose(point=-1.*np.ones(3)))
path = plan_joint_motion(robot, joints, final_conf, obstacles=obstacles,
self_collisions=False, custom_limits=custom_limits)
if path is None:
disconnect()
return
for i, conf in enumerate(path):
set_joint_positions(robot, joints, conf)
point, quat = get_link_pose(robot, body_link)
#euler = euler_from_quat(quat)
euler = intrinsic_euler_from_quat(quat)
print(conf)
print(point, euler)
wait_for_user('Step: {}/{}'.format(i, len(path)))
wait_for_user('Finish?')
disconnect()
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 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 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 wild_gen_fn(name, conf1, conf2, *args):
is_initial = (conf1.element is None) and (conf2.element is not None)
is_goal = (conf1.element is not None) and (conf2.element is None)
if is_initial:
supporters = []
elif is_goal:
supporters = list(element_bodies)
else:
supporters = [conf1.element
] # TODO: can also do according to levels
retrace_supporters(conf1.element, incoming_supporters, supporters)
element_obstacles = {element_bodies[e] for e in supporters}
obstacles = set(static_obstacles) | element_obstacles
if not collisions:
obstacles = set()
robot = index_from_name(robots, name)
conf1.assign()
joints = get_movable_joints(robot)
# TODO: break into pieces at the furthest part from the structure
weights = JOINT_WEIGHTS
resolutions = np.divide(RESOLUTION * np.ones(weights.shape), weights)
disabled_collisions = get_disabled_collisions(robot)
#path = [conf1, conf2]
path = plan_joint_motion(robot,
joints,
conf2.positions,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
weights=weights,
resolutions=resolutions,
restarts=3,
iterations=100,
smooth=100)
if not path:
return
path = [conf1.positions] + path[1:-1] + [conf2.positions]
traj = MotionTrajectory(robot, joints, path)
command = Command([traj])
edges = [
(conf1, command, conf2),
(conf2, command, conf1), # TODO: reverse
]
outputs = []
#outputs = [(command,)]
facts = []
for q1, cmd, q2 in edges:
facts.extend([
('Traj', name, cmd),
('CTraj', name, cmd),
('MoveAction', name, q1, q2, cmd),
])
yield WildOutput(outputs, facts)
def test_kinematic(robot, door, target_x):
wait_if_gui('Begin?')
robot_joints = get_movable_joints(robot)[:3]
joint = robot_joints[0]
start_x = get_joint_position(robot, joint)
num_steps = int(math.ceil(abs(target_x - start_x) / 1e-2))
for x in interpolate(start_x, target_x, num_steps=num_steps):
set_joint_position(robot, joint=joint, value=x)
#with LockRenderer():
solve_collision_free(door, robot, draw=False)
wait_for_duration(duration=1e-2)
#wait_if_gui()
wait_if_gui('Finish?')
def get_ik_fn(robot, fixed=[], teleport=False, num_attempts=10, self_collisions=True):
movable_joints = get_track_arm_joints(robot) if USE_IKFAST else get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
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(gripper_pose, length=0.04)
draw_pose(approach_pose, length=0.04)
for _ in range(num_attempts):
if USE_IKFAST:
q_approach = sample_tool_ik(robot, approach_pose)
if q_approach is not None:
set_joint_positions(robot, movable_joints, q_approach)
else:
set_joint_positions(robot, movable_joints, 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):
continue
conf = BodyConf(robot, joints=movable_joints)
if USE_IKFAST:
q_grasp = sample_tool_ik(robot, gripper_pose, closest_only=True)
if q_grasp is not None:
set_joint_positions(robot, movable_joints, q_grasp)
else:
q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose)
if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles):
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, conf.joints, q_grasp, obstacles=obstacles, \
self_collisions=self_collisions)
if path is None:
if DEBUG_FAILURE: user_input('Approach motion failed')
continue
command = Command([BodyPath(robot, path, joints=movable_joints),
Attach(body, robot, grasp.link),
BodyPath(robot, path[::-1], joints=movable_joints, attachments=[grasp])])
return (conf, command)
# TODO: holding collisions
return None
return fn
def test_door(door):
door_joints = get_movable_joints(door)
sample_fn = get_sample_fn(door, door_joints)
set_joint_positions(door, door_joints, sample_fn())
while True:
positions = sample_fn()
set_joint_positions(door, door_joints, positions)
wait_if_gui()
lower, upper = get_joint_intervals(door, door_joints)
control_joints(door, door_joints, positions=lower)
velocity_control_joints(door, door_joints,
velocities=[PI / 4]) # Able to exceed limits
def plan_approach(end_effector,
print_traj,
collision_fn,
approach_distance=APPROACH_DISTANCE):
# TODO: collisions at the ends of elements
if approach_distance == 0:
return Command([print_traj])
robot = end_effector.robot
joints = get_movable_joints(robot)
extend_fn = get_extend_fn(robot,
joints,
resolutions=0.25 * JOINT_RESOLUTIONS)
tool_link = link_from_name(robot, TOOL_LINK)
approach_pose = Pose(Point(z=-approach_distance))
#element = print_traj.element
#midpoint = get_point(element)
#points = map(point_from_pose, [print_traj.tool_path[0], print_traj.tool_path[-1]])
#midpoint = np.average(list(points), axis=0)
#draw_point(midpoint)
#element_to_base = 0.05*get_unit_vector(get_point(robot) - midpoint)
#retreat_pose = Pose(Point(element_to_base))
# TODO: perpendicular to element
# TODO: solve_ik
start_conf = print_traj.path[0]
set_joint_positions(robot, joints, start_conf)
initial_pose = multiply(print_traj.tool_path[0], approach_pose)
#draw_pose(initial_pose)
#wait_if_gui()
initial_conf = inverse_kinematics(robot, tool_link, initial_pose)
if initial_conf is None:
return None
initial_path = [initial_conf] + list(extend_fn(initial_conf, start_conf))
if any(map(collision_fn, initial_path)):
return None
initial_traj = MotionTrajectory(robot, joints, initial_path)
end_conf = print_traj.path[-1]
set_joint_positions(robot, joints, end_conf)
final_pose = multiply(print_traj.tool_path[-1], approach_pose)
final_conf = inverse_kinematics(robot, tool_link, final_pose)
if final_conf is None:
return None
final_path = [end_conf] + list(extend_fn(
end_conf, final_conf)) # TODO: version that includes the endpoints
if any(map(collision_fn, final_path)):
return None
final_traj = MotionTrajectory(robot, joints, final_path)
return Command([initial_traj, print_traj, final_traj])
def get_element_collision_fn(robot, obstacles):
joints = get_movable_joints(robot)
disabled_collisions = get_disabled_collisions(robot)
custom_limits = {
} # get_custom_limits(robot) # specified within the kuka URDF
robot_links = get_all_links(robot) # Base link isn't real
#robot_links = get_links(robot)
collision_fn = get_collision_fn(robot,
joints,
obstacles=[],
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits=custom_limits)
# TODO: precompute bounding boxes and manually check
#for body in obstacles: # Calling before get_bodies_in_region does not act as step_simulation
# get_aabb(body, link=None)
step_simulation() # Okay to call only once and then just ignore the robot
# TODO: call this once globally
def element_collision_fn(q):
if collision_fn(q):
return True
#step_simulation() # Might only need to call this once
for robot_link in robot_links:
#bodies = obstacles
aabb = get_aabb(robot, link=robot_link)
bodies = {
b
for b, _ in get_bodies_in_region(aabb) if b in obstacles
}
#set_joint_positions(robot, joints, q) # Need to reset
#draw_aabb(aabb)
#print(robot_link, get_link_name(robot, robot_link), len(bodies), aabb)
#print(sum(pairwise_link_collision(robot, robot_link, body, link2=0) for body, _ in region_bodies))
#print(sum(pairwise_collision(robot, body) for body, _ in region_bodies))
#wait_for_user()
if any(
pairwise_link_collision(
robot, robot_link, body, link2=BASE_LINK)
for body in bodies):
#wait_for_user()
return True
return False
return element_collision_fn
def test_ik(robot, node_order, node_points):
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for n in node_order:
point = node_points[n]
set_joint_positions(robot, movable_joints, sample_fn())
conf = inverse_kinematics(robot, link, (point, None))
if conf is not None:
set_joint_positions(robot, movable_joints, conf)
continue
link_point, link_quat = get_link_pose(robot, link)
#print(point, link_point)
#user_input('Continue?')
wait_for_user()
def solve_ik(end_effector, target_pose, nearby=True):
robot = end_effector.robot
movable_joints = get_movable_joints(robot)
if USE_IKFAST:
# TODO: sample from the set of solutions
conf = sample_tool_ik(robot,
target_pose,
closest_only=nearby,
get_all=False)
else:
# TODO: repeat several times
# TODO: condition on current config
if not nearby:
# randomly sample and set joint conf for the pybullet ik fn
sample_fn = get_sample_fn(robot, movable_joints)
set_joint_positions(robot, movable_joints, sample_fn())
# note that the conf get assigned inside this ik fn right away!
conf = inverse_kinematics(robot, end_effector.tool_link, target_pose)
return conf
def is_approach_safe(world, obj_name, pose, grasp, obstacles):
assert pose.support is not None
obj_body = world.get_body(obj_name)
pose.assign() # May set the drawer confs as well
set_joint_positions(world.gripper, get_movable_joints(world.gripper),
world.open_gq.values)
#set_renderer(enable=True)
for _ in iterate_approach_path(world, pose, grasp, body=obj_body):
#for link in get_all_links(world.gripper):
# set_color(world.gripper, apply_alpha(np.zeros(3)), link)
#wait_for_user()
if any(
pairwise_collision(world.gripper, obst
) # or pairwise_collision(obj_body, obst)
for obst in obstacles):
print('Unsafe approach!')
#wait_for_user()
return False
return True
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 test_simulation(robot, target_x, video=None):
use_turtlebot = (get_body_name(robot) == 'turtlebot')
if not use_turtlebot:
target_point, target_quat = map(list, get_pose(robot))
target_point[0] = target_x
add_pose_constraint(robot,
pose=(target_point, target_quat),
max_force=200) # TODO: velocity constraint?
else:
# p.changeDynamics(robot, robot_joints[0], # Doesn't work
# maxJointVelocity=1,
# jointLimitForce=1,)
robot_joints = get_movable_joints(robot)[:3]
print('Max velocities:', get_max_velocities(robot, robot_joints))
print('Max forces:', get_max_forces(robot, robot_joints))
control_joint(robot,
joint=robot_joints[0],
position=target_x,
velocity=0,
position_gain=None,
velocity_scale=None,
max_velocity=100,
max_force=300)
# control_joints(robot, robot_joints, positions=[target_x, 0, PI], max_force=300)
# velocity_control_joints(robot, robot_joints, velocities=[-2., 0, 0]) #, max_force=300)
robot_link = get_first_link(robot)
if video is None:
wait_if_gui('Begin?')
simulate(controller=condition_controller(
lambda *args: abs(target_x - point_from_pose(
get_link_pose(robot, robot_link))[0]) < 1e-3),
sleep=0.01) # TODO: velocity condition
# print('Velocities:', get_joint_velocities(robot, robot_joints))
# print('Torques:', get_joint_torques(robot, robot_joints))
if video is None:
set_renderer(enable=True)
wait_if_gui('Finish?')
def 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 main(use_turtlebot=True):
parser = argparse.ArgumentParser()
parser.add_argument('-sim', action='store_true')
parser.add_argument('-video', action='store_true')
args = parser.parse_args()
video = 'video.mp4' if args.video else None
connect(use_gui=True, mp4=video)
#set_renderer(enable=False)
# print(list_pybullet_data())
# print(list_pybullet_robots())
draw_global_system()
set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5),
target_point=Point(-1.5, +1.5, 0))
plane = load_plane()
#door = load_pybullet('models/door.urdf', fixed_base=True) # From drake
#set_point(door, Point(z=-.1))
door = create_door()
#set_position(door, z=base_aligned_z(door))
set_point(door, base_aligned(door))
#set_collision_margin(door, link=0, margin=0.)
set_configuration(door, [math.radians(-5)])
dump_body(door)
door_joint = get_movable_joints(door)[0]
door_link = child_link_from_joint(door_joint)
#draw_pose(get_com_pose(door, door_link), parent=door, parent_link=door_link)
draw_pose(Pose(), parent=door, parent_link=door_link)
wait_if_gui()
##########
start_x = +2
target_x = -start_x
if not use_turtlebot:
side = 0.25
robot = create_box(w=side, l=side, h=side, mass=5., color=BLUE)
set_position(robot, x=start_x)
#set_velocity(robot, linear=Point(x=-1))
else:
turtlebot_urdf = os.path.abspath(TURTLEBOT_URDF)
print(turtlebot_urdf)
#print(read(turtlebot_urdf))
robot = load_pybullet(turtlebot_urdf, merge=True, fixed_base=True)
robot_joints = get_movable_joints(robot)[:3]
set_joint_positions(robot, robot_joints, [start_x, 0, PI])
set_all_color(robot, BLUE)
set_position(robot, z=base_aligned_z(robot))
dump_body(robot)
##########
set_renderer(enable=True)
#test_door(door)
if args.sim:
test_simulation(robot, target_x, video)
else:
assert use_turtlebot # TODO: extend to the block
test_kinematic(robot, door, target_x)
disconnect()
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
