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: