def traj_collisions(sim_env, full_traj, collision_dist_threshold, n=100):
"""
Returns the set of collisions.
manip = Manipulator or list of indices
"""
traj, dof_inds = full_traj
traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)),
traj)
cc = trajoptpy.GetCollisionChecker(sim_env.env)
with openravepy.RobotStateSaver(sim_env.robot):
sim_env.robot.SetActiveDOFs(dof_inds)
col_times = []
for (i, row) in enumerate(traj_up):
sim_env.robot.SetActiveDOFValues(row)
col_now = cc.BodyVsAll(sim_env.robot)
#with util.suppress_stdout():
# col_now2 = cc.PlotCollisionGeometry()
col_now = [
cn for cn in col_now
if cn.GetDistance() < collision_dist_threshold
]
if col_now:
#print [cn.GetDistance() for cn in col_now]
col_times.append(i)
#print "trajopt.CollisionChecker: ", len(col_now)
#print col_now2
return col_times
def create(self, rope_pts):
self.bt_env = bulletsimpy.BulletEnvironment(self.env, [])
self.bt_env.SetGravity([0, 0, -9.8])
self.bt_robot = self.bt_env.GetObjectByName(self.robot.GetName())
capsule_rope_params = bulletsimpy.CapsuleRopeParams()
capsule_rope_params.radius = self.rope_params.radius
capsule_rope_params.angStiffness = self.rope_params.angStiffness
capsule_rope_params.angDamping = self.rope_params.angDamping
capsule_rope_params.linDamping = self.rope_params.linDamping
capsule_rope_params.angLimit = self.rope_params.angLimit
capsule_rope_params.linStopErp = self.rope_params.linStopErp
capsule_rope_params.mass = self.rope_params.mass
self.rope = bulletsimpy.CapsuleRope(self.bt_env, 'rope', rope_pts,
capsule_rope_params)
self.rope_pts = rope_pts
cc = trajoptpy.GetCollisionChecker(self.env)
for gripper_link in [
link for link in self.robot.GetLinks()
if 'gripper' in link.GetName()
]:
for rope_link in self.rope.GetKinBody().GetLinks():
cc.ExcludeCollisionPair(gripper_link, rope_link)
# self.rope.UpdateRave()
# self.env.UpdatePublishedBodies()
# trajoptpy.GetViewer(self.env).Idle()
self.settle()
def __init__(self, view=False):
env_file = os.path.join(rll_quadrotor_folder,
'simulation/models/trajopt_quadrotor.xml') # TODO
self.env = rave.Environment()
physics = rave.RaveCreatePhysicsEngine(self.env,'bullet')
self.env.SetPhysicsEngine(physics)
self.env.StopSimulation()
self.env.Load(env_file)
self.kinbodies = set([b for b in self.env.GetBodies() if not b.IsRobot()])
self.handles = list()
self.view = view
self.viewer = None
if view:
self.env.SetViewer('qtcoin')
self.viewer = self.env.GetViewer()
time.sleep(0.1)
assert(len(self.env.GetRobots()) == 1)
self.robot = self.env.GetRobots()[0]
self.cc = trajoptpy.GetCollisionChecker(self.env)
self.aabb_cache = dict()
self.is_watertight_cache = dict()
def remove_from_env(self):
# remove all capsule-capsule exclude to prevent memory leak
# TODO: only interate through the capsule pairs that actually are excluded
cc = trajoptpy.GetCollisionChecker(self.sim.env)
for rope_link0 in self.rope.GetKinBody().GetLinks():
for rope_link1 in self.rope.GetKinBody().GetLinks():
cc.IncludeCollisionPair(rope_link0, rope_link1)
self.sim.env.Remove(self.rope.GetKinBody())
self.rope = None
self.sim = None
def _include_gripper_finger_collisions(self):
if not self.robot:
return
cc = trajoptpy.GetCollisionChecker(self.env)
for lr in 'lr':
for flr in 'lr':
finger_link_name = "%s_gripper_%s_finger_tip_link" % (lr, flr)
finger_link = self.robot.GetLink(finger_link_name)
for sim_obj in self.sim_objs:
for bt_obj in sim_obj.get_bullet_objects():
for link in bt_obj.GetKinBody().GetLinks():
cc.IncludeCollisionPair(finger_link, link)
def postsetup_trajopt(env):
"use the ROS config file to ignore some impossible self collisions. very slight speedup"
import xml.etree.ElementTree as ET
robot = env.GetRobot("pr2")
cc = trajoptpy.GetCollisionChecker(env)
root = ET.parse(
"/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot()
disabled_elems = root.findall("disable_collisions")
for elem in disabled_elems:
linki = robot.GetLink(elem.get("link1"))
linkj = robot.GetLink(elem.get("link2"))
if linki and linkj:
cc.ExcludeCollisionPair(linki, linkj)
def __del__(self):
if self.rope:
cc = trajoptpy.GetCollisionChecker(self.env)
for gripper_link in [
link for link in self.robot.GetLinks()
if 'gripper' in link.GetName()
]:
for rope_link in self.rope.GetKinBody().GetLinks():
cc.IncludeCollisionPair(gripper_link, rope_link)
# remove all capsule-capsule exclude to prevent memory leak
# TODO: only interate through the capsule pairs that actually are excluded
for rope_link0 in self.rope.GetKinBody().GetLinks():
for rope_link1 in self.rope.GetKinBody().GetLinks():
cc.IncludeCollisionPair(rope_link0, rope_link1)
self.env.Remove(self.rope.GetKinBody())
self.rope = None
self.rope_pts = None
def exclude_gripper_collisions(opposite=False, fingertips_only=False):
cc = trajoptpy.GetCollisionChecker(env)
links = []
for lr in 'lr':
if not fingertips_only:
links.append(robot.GetLink("%s_wrist_flex_link" % (lr)))
links.append(robot.GetLink("%s_wrist_roll_link" % (lr)))
links.append(robot.GetLink("%s_gripper_palm_link" % (lr)))
for flr in 'lr':
links.append(robot.GetLink("%s_gripper_%s_finger_tip_link" % (lr, flr)))
if not fingertips_only:
links.append(robot.GetLink("%s_gripper_%s_finger_link" % (lr, flr)))
for r_link in links:
for kin_body in kin_bodies:
for link in kin_body.GetLinks():
if not opposite:
cc.ExcludeCollisionPair(r_link, link)
else:
cc.IncludeCollisionPair(r_link, link)
def load_simulation(args, sim_env):
sim_env.env = openravepy.Environment()
sim_env.env.StopSimulation()
sim_env.env.Load("robots/pr2-beta-static.zae")
sim_env.robot = sim_env.env.GetRobots()[0]
actions = h5py.File(args.actionfile, 'r')
fakedatafile = h5py.File(args.fakedatafile, 'r')
GlobalVars.init_tfm = fakedatafile['init_tfm'][()]
init_rope_xyz, _ = sim_util.load_fake_data_segment(
sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform
) # this also sets the torso (torso_lift_joint) to the height in the data
# Set table height to correct height of first rope in holdout set
holdoutfile = h5py.File(args.holdoutfile, 'r')
first_holdout = holdoutfile[holdoutfile.keys()[0]]
init_rope_xyz = first_holdout['rope_nodes'][:]
if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r':
init_rope_xyz = init_rope_xyz.dot(
GlobalVars.init_tfm[:3, :3].T) + GlobalVars.init_tfm[:3,
3][None, :]
table_height = init_rope_xyz[:, 2].mean() - .02 # Before: .02
table_xml = sim_util.make_table_xml(translation=[1, 0, table_height],
extents=[.85, .55, .01])
sim_env.env.LoadData(table_xml)
if 'bookshelve' in args.obstacles:
sim_env.env.Load("data/bookshelves.env.xml")
if 'boxes' in args.obstacles:
sim_env.env.LoadData(
sim_util.make_box_xml("box0",
[.7, .43, table_height + (.01 + .12)],
[.12, .12, .12]))
sim_env.env.LoadData(
sim_util.make_box_xml(
"box1", [.74, .47, table_height + (.01 + .12 * 2 + .08)],
[.08, .08, .08]))
cc = trajoptpy.GetCollisionChecker(sim_env.env)
for gripper_link in [
link for link in sim_env.robot.GetLinks()
if 'gripper' in link.GetName()
]:
cc.ExcludeCollisionPair(gripper_link,
sim_env.env.GetKinBody('table').GetLinks()[0])
sim_util.reset_arms_to_side(sim_env)
if args.animation:
sim_env.viewer = trajoptpy.GetViewer(sim_env.env)
if args.animation > 1 and os.path.isfile(
args.window_prop_file) and os.path.isfile(
args.camera_matrix_file):
print "loading window and camera properties"
window_prop = np.loadtxt(args.window_prop_file)
camera_matrix = np.loadtxt(args.camera_matrix_file)
try:
sim_env.viewer.SetWindowProp(*window_prop)
sim_env.viewer.SetCameraManipulatorMatrix(camera_matrix)
except:
print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
else:
print "move viewer to viewpoint that isn't stupid"
print "then hit 'p' to continue"
sim_env.viewer.Idle()
print "saving window and camera properties"
try:
window_prop = sim_env.viewer.GetWindowProp()
camera_matrix = sim_env.viewer.GetCameraManipulatorMatrix()
np.savetxt(args.window_prop_file, window_prop, fmt='%d')
np.savetxt(args.camera_matrix_file, camera_matrix)
except:
print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
robot = env.GetRobots()[0]
init_transform = np.eye(4)
init_transform[:3,3] = [-.35, 1, .92712]
init_transform[:3,3] = [.1, 1, .92712]
#init_transform[:3,3] = [2.6, 1, .92712]
robot.SetTransform(init_transform)
robot.SetDOFValues(np.zeros(robot.GetDOF()))
robot.SetActiveDOFs(np.arange(robot.GetDOF()), rave.DOFAffine.Transform)
# move arms to side
robot.SetDOFValues([-1.3],[robot.GetJoint("l_arm_shx").GetDOFIndex()])
robot.SetDOFValues([1.3],[robot.GetJoint("r_arm_shx").GetDOFIndex()])
standing_posture = robot.GetActiveDOFValues()
##################
trajoptpy.SetInteractive(True)
cc = trajoptpy.GetCollisionChecker(env)
cc.ExcludeCollisionPair(robot.GetLink("l_foot"), env.GetKinBody("ProjectRoom").GetLink("Floor"))
cc.ExcludeCollisionPair(robot.GetLink("r_foot"), env.GetKinBody("ProjectRoom").GetLink("Floor"))
n_steps = 5
x_button_press = 2.55 # if robot is at this x-coordinate, he can reach the button
xyz_button = env.GetKinBody("bigredbutton").GetTransform()[:3,3]
xyz_button[2] += .15;
totaltraj = []
i=0
while robot.GetTransform()[0,3] < x_button_press: