def _setup_self_collision_fn(self, sawyer_robot):
sawyer_id = self.sawyer_robot.get_simulator_id()
excluded_pairs = [
(get_joint_info_by_name(sawyer_id, 'right_l6').idx,
get_joint_info_by_name(sawyer_id,
'right_connector_plate_base').idx)
]
link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs)
return partial(self_collision_test,
robot=sawyer_robot,
link_pairs=link_pairs)
def _setup_collision_exclusions(self):
ground_plane = self.get_sim_objects(['Ground'])[0]
sawyer_id = self.sawyer_robot.get_simulator_id()
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id,
'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
self.collision_exclusions = {
"excluded_bodies": excluded_bodies,
"excluded_body_link_pairs": excluded_body_link_pairs
}
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)
def main():
################################
# Environment Checks and Flags #
################################
if os.environ.get('ROS_DISTRO'):
rospy.init_node("CAIRO_Sawyer_Simulator")
use_ros = True
else:
use_ros = False
########################################################
# Create the Simulator and pass it a Logger (optional) #
########################################################
logger = Logger()
sim = Simulator(logger=logger,
use_ros=use_ros,
use_gui=True,
use_real_time=True) # Initialize the Simulator
#####################################
# Create a Robot, or two, or three. #
#####################################
sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1)
#############################################
# Create sim environment objects and assets #
#############################################
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
sawyer_id = sawyer_robot.get_simulator_id()
sawyer_robot.move_to_joint_pos([
0.006427734375, -0.4784267578125, -2.6830537109375, -1.5901376953125,
0.1734560546875, 1.1468447265625, 1.310236328125
])
time.sleep(3)
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
############
# PLANNING #
############
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies,
excluded_body_link_pairs):
#########################
# STATE SPACE SELECTION #
#########################
# This inherently uses UniformSampler but a different sampling class could be injected.
state_space = SawyerConfigurationSpace()
##############################
# STATE VALIDITY FORMULATION #
##############################
# Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision.
excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx,
get_joint_info_by_name(sawyer_id, "right_l0").idx),
(get_joint_info_by_name(sawyer_id, "right_l1_2").idx,
get_joint_info_by_name(sawyer_id, "head").idx)]
link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs)
self_collision_fn = partial(self_collision_test,
robot=sawyer_robot,
link_pairs=link_pairs)
# Create constraint checks
def constraint_test(q):
upright_orientation = [
0.0005812598018143569, 0.017721236427960724,
-0.6896867930096543, 0.723890701324838
]
axis = 'z'
threshold = 15
world_pose, _ = sawyer_robot.solve_forward_kinematics(q)
return orientation(upright_orientation, world_pose[1], threshold,
axis)
# In this case, we only have a self_col_fn.
svc = StateValidityChecker(self_col_func=self_collision_fn,
col_func=None,
validity_funcs=[constraint_test])
#######
# PRM #
#######
# Use parametric linear interpolation with 5 steps between points.
interp = partial(parametric_lerp, steps=5)
# See params for PRM specific parameters
prm = PRM(state_space,
svc,
interp,
params={
'n_samples': 1000,
'k': 10,
'ball_radius': 3.0
})
logger.info("Planning....")
plan = prm.plan(
np.array([
0.006427734375, -0.4784267578125, -2.6830537109375,
-1.5901376953125, 0.1734560546875, 1.1468447265625,
1.310236328125
]),
np.array([
-0.9232412109375, 0.2353603515625, -2.51373828125,
-0.6898984375, 0.33058203125, 1.0955361328125, 1.14510546875
]))
logger.info("Plan found....")
print(len(plan))
print(plan)
# 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)
##########
# SPLINE #
##########
# 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=5)
print("Executing splined trajectory...")
sawyer_robot.execute_trajectory(traj)
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)
def parallel_sample_worker(num_samples):
########################################################
# Create the Simulator and pass it a Logger (optional) #
########################################################
logger = Logger()
if not Simulator.is_instantiated():
logger.warn("Simulator not instantiated, doing so now...")
sim = Simulator(logger=logger, use_ros=False, use_gui=False,
use_real_time=True) # Initialize the Simulator
else:
sim = Simulator.get_instance()
#####################################
# Create a Robot, or two, or three. #
#####################################
sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1)
#############################################
# Create sim environment objects and assets #
#############################################
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
sawyer_id = sawyer_robot.get_simulator_id()
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
############
# SAMPLING #
############
valid_samples = []
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs):
#########################
# STATE SPACE SELECTION #
#########################
# This inherently uses UniformSampler but a different sampling class could be injected.
state_space = SawyerConfigurationSpace()
##############################
# STATE VALIDITY FORMULATION #
##############################
# Certain links in Sawyer seem to be permentently in self collision. This is how to remove them by name when getting all link pairs to check for self collision.
excluded_pairs = [(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "right_l0").idx),
(get_joint_info_by_name(sawyer_id, "right_l1_2").idx, get_joint_info_by_name(sawyer_id, "head").idx)]
link_pairs = get_link_pairs(sawyer_id, excluded_pairs=excluded_pairs)
self_collision_fn = partial(
self_collision_test, robot=sawyer_robot, link_pairs=link_pairs)
# Create constraint checks
# def constraint_test(q):
# upright_orientation = [
# 0.0005812598018143569, 0.017721236427960724, -0.6896867930096543, 0.723890701324838]
# axis = 'z'
# threshold = 15
# world_pose, _ = sawyer_robot.solve_forward_kinematics(q)
# return orientation(upright_orientation, world_pose[1], threshold, axis)
# In this case, we only have a self_col_fn.
svc = StateValidityChecker(
self_col_func=self_collision_fn, col_func=None, validity_funcs=None)
# svc = StateValidityChecker(
# self_col_func=self_collision_fn, col_func=None, validity_funcs=[constraint_test])
count = 0
while count < num_samples:
q_rand = np.array(state_space.sample())
if svc.validate(q_rand):
valid_samples.append(q_rand)
count += 1
return valid_samples
use_real_time=True) # Initialize the Simulator
#####################################
# Create a Robot, or two, or three. #
#####################################
sawyer_robot = Sawyer("sawyer0", [0, 0, 0.9], fixed_base=1)
sawyer_id = sawyer_robot.get_simulator_id()
#############################################
# Create sim environment objects and assets #
#############################################
ground_plane = SimObject("Ground", "plane.urdf", [0, 0, 0])
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
############
# PLANNING #
############
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies, excluded_body_link_pairs):
#########################
# STATE SPACE SELECTION #
#########################
# This inherently uses UniformSampler but a different sampling class could be injected.
state_space = SawyerConfigurationSpace()
##############################
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_id = sawyer_robot.get_simulator_id()
ground_plane = sim_context.get_sim_objects(['Ground'])[0]
# Exclude the ground plane and the pedestal feet from disabled collisions.
excluded_bodies = [ground_plane.get_simulator_id()] # the ground plane
pedestal_feet_idx = get_joint_info_by_name(sawyer_id, 'pedestal_feet').idx
# The (sawyer_idx, pedestal_feet_idx) tuple the ecluded from disabled collisions.
excluded_body_link_pairs = [(sawyer_id, pedestal_feet_idx)]
############
# PLANNING #
############
# Disabled collisions during planning with certain eclusions in place.
with DisabledCollisionsContext(sim, excluded_bodies,
excluded_body_link_pairs):
#######
# PRM #
#######
# Use parametric linear interpolation with 10 steps between points.
interp = partial(parametric_lerp, steps=10)
# See params for PRM specific parameters
prm = PRM(state_space,
svc,
interp,
params={
'n_samples': 3000,
'k': 6,
'ball_radius': 2.0
})
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....")
print(len(path))
##########
# SPLINE #
##########
# 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=10)
sawyer_robot.execute_trajectory(traj)
try:
while True:
sim.step()
except KeyboardInterrupt:
p.disconnect()
sys.exit(0)