def main():
sim_context = SawyerSimContext()
sim = sim_context.get_sim_instance()
logger = sim_context.get_logger()
state_space = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
_ = sim_context.get_sim_objects(['Ground'])[0]
sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0])
time.sleep(1)
with DisabledCollisionsContext(sim, [], []):
#######
# LazyPRM #
#######
# Use parametric linear interpolation with 10 steps between points.
interp = partial(parametric_lerp, steps=10)
# See params for PRM specific parameters
prm = LazyPRM(state_space,
svc,
interp,
params={
'n_samples': 4000,
'k': 8,
'ball_radius': 2.5
})
logger.info("Planning....")
plan = prm.plan(
np.array([0, 0, 0, 0, 0, 0, 0]),
np.array([
1.5262755737449423, -0.1698540226273928, 2.7788151824762055,
2.4546623466066135, 0.7146948867821279, 2.7671787952787184,
2.606128412644311
]))
# get_path() reuses the interp function to get the path between vertices of a successful plan
path = prm.get_path(plan)
if len(path) == 0:
logger.info("Planning failed....")
sys.exit(1)
logger.info("Plan found....")
# splinging uses numpy so needs to be converted
path = [np.array(p) for p in path]
# Create a MinJerk spline trajectory using JointTrajectoryCurve and execute
jtc = JointTrajectoryCurve()
traj = jtc.generate_trajectory(path, move_time=2)
sawyer_robot.execute_trajectory(traj)
key = input("Press any key to excute plan.")
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
def main():
sim_context = SawyerSimContext()
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
state_space = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
_ = sim_context.get_sim_objects(['Ground'])[0]
sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0])
time.sleep(1)
############
# PLANNING #
############
valid_samples = []
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, [], []):
while True:
sample = state_space.sample()
if svc.validate(sample):
valid_samples.append(sample)
if len(valid_samples) >= 1:
break
# Generate local plan between two points and execute local plan.
steps = 100
move_time = 5
start_pos = [0] * 7
print(np.array(sawyer_robot.get_current_joint_states()[0:7]))
print(np.array(valid_samples[0]))
qt, qdt, qddt = interpolate_5poly(np.array(start_pos[0:7]),
np.array(valid_samples[0]), steps)
traj = list(
zip([move_time * n / steps for n in range(0, steps)],
[list(q) for q in qt]))
print(traj)
sawyer_robot.execute_trajectory(traj)
# Loop until someone shuts us down
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
def main():
sim_context = SawyerSimContext()
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
state_space = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
_ = sim_context.get_sim_objects(['Ground'])[0]
sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0])
time.sleep(1)
valid_samples = []
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, [], []):
while True:
sample = state_space.sample()
if svc.validate(sample):
valid_samples.append(sample)
if len(valid_samples) >= 1:
break
# Generate local plan between two points and execute local plan.
start_pos = [0] * 7
sawyer_robot.move_to_joint_pos(start_pos)
time.sleep(.1)
# This interpolate_5poly will be replaced with the sequence of points generated by a planner.
qt, _, _ = interpolate_5poly(np.array(start_pos[0:7]),
np.array(valid_samples[0]), 50)
jtc = JointTrajectoryCurve()
traj = jtc.generate_trajectory(qt, move_time=2)
sawyer_robot.execute_trajectory(traj)
# Loop until someone shuts us down
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
def main():
config = {}
config["sim_objects"] = [{
"object_name": "Ground",
"model_file_or_sim_id": "plane.urdf",
"position": [0, 0, 0]
}, {
"object_name": "Table",
"model_file_or_sim_id": ASSETS_PATH + 'table.sdf',
"position": [0.0, 0, 0],
"orientation": [0, 0, 1.5708]
}]
sim_context = SawyerSimContext(config)
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
scs = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
table_obj = sim_context.get_sim_objects(names="Table")[0]
sawyer_robot.move_to_joint_pos([0, 0, 0, 0, 0, 0, 0])
time.sleep(1)
n_samples = 1000
valid_samples = []
starttime = timeit.default_timer()
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, [], []):
print("Sampling start time is :", starttime)
for i in range(0, n_samples):
sample = scs.sample()
if svc.validate(sample):
print(sample)
valid_samples.append(sample)
print("The time difference is :", timeit.default_timer() - starttime)
print("{} valid of {}".format(len(valid_samples), n_samples))
p.disconnect()
def main():
sim_context = SawyerSimContext(setup=False)
sim_context.setup(sim_overrides={"use_gui": False})
sim = sim_context.get_sim_instance()
logger = sim_context.get_logger()
state_space = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
sawyer_id = sawyer_robot.get_simulator_id()
ground_plane = sim_context.get_sim_objects(['Ground'])[0]
# Get the robot moving!
sawyer_robot.move_to_joint_pos(target_position=[1] * 7)
while sawyer_robot.check_if_at_position([1] * 7, 0.5) is False:
p.stepSimulation()
print('\n\n\n\n\n\n')
# Get current states
pos, vel, torq = getJointStates(sawyer_id)
names, mpos, mvel, mtorq = getMotorJointStates(sawyer_id)
print("Moving joint names:")
print(names)
sawyerEELink = get_joint_info_by_name(sawyer_id, 'right_l6').idx
print(sawyerEELink)
result = p.getLinkState(sawyer_id,
sawyerEELink,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(sawyer_id, sawyerEELink, com_trn,
mpos, zero_vec, zero_vec)
J = np.vstack([np.array(jac_t),
np.array(jac_r)])[:, [0, 2, 3, 4, 5, 6, 7]]
print("JACOBIAN")
print(J)
J_cross = np.dot(J.T, np.linalg.inv(np.dot(J, J.T)))
print("PSEUDOINVERSE JACOBIAN")
print(J_cross)
print("Link linear position and velocity of CoM from getLinkState:")
print(link_vt, link_vr)
print(
"Link linear and angular velocity of CoM from linearJacobian * q_dot:"
)
print(np.dot(J, np.array(mvel)[[0, 2, 3, 4, 5, 6, 7]].T))
print()
print()
print("Link joint velocities of CoM from getLinkState:")
print(np.array(mvel)[[0, 2, 3, 4, 5, 6, 7]])
print(
"Link q given link linear position and velocity using J_cross * x_dot."
)
x = np.hstack([np.array(link_vt), np.array(link_vr)])
q = np.dot(J_cross, np.array(x).T)
print(q)
['right_gripper_l_finger_joint', (0.0, 0.020833)],
['right_gripper_r_finger_joint', (-0.020833, 0.0)],
['head_pan', (-5.0952, 0.9064)]]
#############################################
# Import Serialized LfD Graph #
#############################################
with open(
os.path.dirname(os.path.abspath(__file__)) +
"/serialization_test.json", "r") as f:
serialized_data = json.load(f)
config = serialized_data["config"]
intermediate_trajectories = serialized_data["intermediate_trajectories"]
keyframes = OrderedDict(
sorted(serialized_data["keyframes"].items(), key=lambda t: int(t[0])))
sim_context = SawyerSimContext(None, setup=False, planning_context=None)
sim_context.setup(sim_overrides={"run_parallel": False})
sim = sim_context.get_sim_instance()
logger = sim_context.get_logger()
sawyer_robot = sim_context.get_robot()
svc = sim_context.get_state_validity()
interp_fn = partial(parametric_lerp, steps=5)
#############################################
# CREATE A PLANNING GRAPH #
#############################################
# This will be used to generate a sequence #
# of constrained motion plans. #
#############################################
planning_G = nx.Graph()
def main():
configuration = {}
configuration["sim"] = {
"use_real_time": False,
"use_gui": False,
"run_parallel": True
}
configuration["logging"] = {"handlers": ['logging'], "level": "debug"}
configuration["sawyer"] = {
"robot_name": "sawyer0",
"position": [0, 0, 0.9],
"fixed_base": True
}
configuration["sim_objects"] = [
{
"object_name": "Ground",
"model_file_or_sim_id": "plane.urdf",
"position": [0, 0, 0]
},
# {
# "object_name": "Table",
# "model_file_or_sim_id": ASSETS_PATH + 'table.sdf',
# "position": [0.7, 0, 0],
# "orientation": [0, 0, 1.5708]
# },
{
"object_name": "cube0",
"model_file_or_sim_id": "cube_small.urdf",
"position": [0.75, 0, .55]
},
{
"object_name": "cube1",
"model_file_or_sim_id": "cube_small.urdf",
"position": [0.74, 0.05, .55]
},
{
"object_name": "cube2",
"model_file_or_sim_id": "cube_small.urdf",
"position": [0.67, -0.1, .55]
},
{
"object_name": "cube3",
"model_file_or_sim_id": "cube_small.urdf",
"position": [0.69, 0.1, .55]
}
]
num_workers = 8
worker_fn = partial(parallel_projection_worker,
sim_context_cls=SawyerSimContext,
sim_config=configuration)
with Pool(num_workers) as p:
multi_s = timer()
results = p.map(worker_fn, [25, 25, 25, 25, 25, 25, 25, 25])
valid_samples = list(itertools.chain.from_iterable(results))
multi_e = timer()
print("Number of valid samples: {}".format(len(valid_samples)))
configuration["sim"] = {
"use_real_time": True,
"use_gui": True,
"run_parallel": False
}
sim_context = SawyerSimContext(configuration)
sim = sim_context.get_sim_instance()
_ = sim_context.get_logger()
scs = sim_context.get_state_space()
svc = sim_context.get_state_validity()
sawyer_robot = sim_context.get_robot()
_ = sawyer_robot.get_simulator_id()
_ = sim_context.get_sim_objects(['Ground'])[0]
for sample in valid_samples:
world_pose, local_pose = sawyer_robot.solve_forward_kinematics(sample)
trans, quat = world_pose[0], world_pose[1]
print(trans, quat2rpy(quat))
sawyer_robot.move_to_joint_pos(list(sample))
while sawyer_robot.check_if_at_position(list(sample), 0.5) is False:
time.sleep(0.1)
pass
time.sleep(1.5)
# # Loop until someone shuts us down
try:
while True:
pass
# sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
##################################
# Planning Context #
##################################
config = {}
config["sim_objects"] = [{
"object_name": "Ground",
"model_file_or_sim_id": "plane.urdf",
"position": [0, 0, 0]
}, {
"object_name": "Table",
"model_file_or_sim_id": ASSETS_PATH + 'table.sdf',
"position": [3.0, 0, 0],
"orientation": [0, 0, 1.5708]
}]
sim_context = SawyerSimContext(config, setup=True, planning_context=None)
sim = sim_context.get_sim_instance()
logger = sim_context.get_logger()
sawyer_robot = sim_context.get_robot()
svc = sim_context.get_state_validity()
interp_fn = partial(parametric_lerp, steps=5)
#############################################
# Import Serialized LfD Graph #
#############################################
with open(
os.path.dirname(os.path.abspath(__file__)) +
"/serialization_test.json", "r") as f:
serialized_data = json.load(f)
config = serialized_data["config"]
intermediate_trajectories = serialized_data["intermediate_trajectories"]