def get_collisions(self, traj, n=100): orig_values = self.robot.GetDOFValues(self.manip.GetArmIndices()); collisions = set() traj_up = mu.interp2d(np.linspace(0,1,n), np.linspace(0,1,len(traj)), traj) with self.env: for joint_values in traj_up: self.robot.SetDOFValues(joint_values, self.manip.GetArmIndices()) for obj in utils.get_all_collisions(self.robot, self.env): collisions.add(obj) self.robot.SetDOFValues(orig_values, self.manip.GetArmIndices()) return collisions
def _num_collisions(self, joint_traj, up_samples=120): traj_up = mu.interp2d(np.linspace(0,1,up_samples), np.linspace(0,1,len(joint_traj)), joint_traj) manip_links = [l for l in self.robot.GetLinks() if l not in self.manip.GetIndependentLinks()] num_collisions = 0 for (i,row) in enumerate(traj_up): self.robot.SetDOFValues(row, self.joint_indices) #is_collision = max([self.sim.env.CheckCollision(l) for l in manip_links]) is_collision = self.sim.env.CheckCollision(self.robot) if is_collision: num_collisions += 1 return num_collisions
def traj_collisions(traj, robot, n=100): """ Returns the set of collisions. manip = Manipulator or list of indices """ traj_up = mu.interp2d(np.linspace(0,1,n), np.linspace(0,1,len(traj)), traj) ss = rave.RobotStateSaver(robot) env = robot.GetEnv() col_times = [] for (i,row) in enumerate(traj_up): robot.SetActiveDOFValues(row) col_now = env.CheckCollision(robot) if col_now: col_times.append(i) return col_times
def traj_collisions(traj, robot, n=100): """ Returns the set of collisions. manip = Manipulator or list of indices """ traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)), traj) ss = rave.RobotStateSaver(robot) env = robot.GetEnv() col_times = [] for (i, row) in enumerate(traj_up): robot.SetActiveDOFValues(row) col_now = env.CheckCollision(robot) if col_now: col_times.append(i) return col_times
def check_traj(traj, manip, n=100): traj_up = mu.interp2d(np.linspace(0,1,n), np.linspace(0,1,len(traj)), traj) robot = manip.GetRobot() ss = rave.RobotStateSaver(robot) arm_inds = manip.GetArmIndices() env = robot.GetEnv() collision = False col_times = [] for (i,row) in enumerate(traj_up): robot.SetDOFValues(row, arm_inds) col_now = env.CheckCollision(robot) if col_now: collision = True col_times.append(i) if col_times: print "collision at timesteps", col_times else: print "no collisions" return collision
def _num_collisions(self, joint_traj, up_samples=120): traj_up = mu.interp2d(np.linspace(0, 1, up_samples), np.linspace(0, 1, len(joint_traj)), joint_traj) manip_links = [ l for l in self.robot.GetLinks() if l not in self.manip.GetIndependentLinks() ] num_collisions = 0 for (i, row) in enumerate(traj_up): self.robot.SetDOFValues(row, self.joint_indices) #is_collision = max([self.sim.env.CheckCollision(l) for l in manip_links]) is_collision = self.sim.env.CheckCollision(self.robot) if is_collision: num_collisions += 1 return num_collisions
def traj_collisions(traj, robot, n=100): """ Returns the set of collisions. manip = Manipulator or list of indices """ traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)), traj) env = robot.GetEnv() col_times = [] with robot: for (i, row) in enumerate(traj_up): robot.SetActiveDOFValues(row) col_env = env.CheckCollision(robot) col_self = robot.CheckSelfCollision() if col_env or col_self: col_times.append(i) return col_times
def check_traj(traj, manip, n=100): traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)), traj) robot = manip.GetRobot() ss = rave.RobotStateSaver(robot) arm_inds = manip.GetArmIndices() env = robot.GetEnv() collision = False col_times = [] for (i, row) in enumerate(traj_up): robot.SetDOFValues(row, arm_inds) col_now = env.CheckCollision(robot) if col_now: collision = True col_times.append(i) if col_times: print "collision at timesteps", col_times else: print "no collisions" return collision
def check_for_collisions_interp(action, problem, T, env, dn=10): """ Verify that the interpolated trajectory generated by trajopt is collision free. Argument 'dn' controls how fine the step sizes between points is. """ # If the action is a pre-grasp, then we do not check the last segment for # collisions. # pregrasp_off = 0 # if is_action_pregrasp(action) and is_action_linear(action): # pregrasp_off = 1 # Disable collisions for items if the action is a finger closing # pre-grasp. Grasp and post-grasp actions have attached the object # to the robot, so collisions are not reported. if is_action_pregrasp(action) and is_action_linear(action): item = env.GetKinBody(action.object_key) item.Enable(False) # TODO Disable collision checking against other objects in the bin. # Check interpolated trajectory for collisions. robot = env.GetRobot("crichton") # T = result.GetTraj() num_pts = T.shape[0] num_dof = T.shape[1] report = openravepy.CollisionReport() for i in range(num_pts): T_i = T[i:i+1,:] T_i = mu.interp2d(np.linspace(0,1,dn), np.linspace(0,1,len(T_i)), T_i) for (_,dofs) in enumerate(T_i): robot.SetActiveDOFValues(dofs) for robot_link in robot.GetLinks(): collision = env.CheckCollision(robot_link, report) if collision: print apc_colors.FAIL + "collision: " + str(report) + apc_colors.ENDC return False # Re-enable collisions for items if the action is a pre-grasp. Grasp # and post-grasp actions have attached the object to the robot, so # collisions are not reported. if is_action_pregrasp(action): item = env.GetKinBody(action.object_key) item.Enable(True) return True
def move_arm_straight_request(manip, n_steps, link_name, xyz_start, xyz_end, quat_start, quat_end, start_joints): manip_name = manip.GetName() request = { "basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": True}, "costs": [ {"type": "joint_vel", "params": {"coeffs": [1]}}, {"type": "collision", "params": {"coeffs": [10], "dist_pen": [0.025]}}, ], "constraints": [ { "type": "pose", "name": "final_pose", "params": { "pos_coeffs": [10, 10, 10], "rot_coeffs": [10, 10, 10], "xyz": list(xyz_end), "wxyz": list(quat_end), "link": link_name, }, } ], "init_info": {}, } xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps) quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)] hmats = [rave.matrixFromPose(np.r_[quat, xyz]) for (xyz, quat) in zip(xyzs, quats)] def ikfunc(hmat): return ku.ik_for_link(hmat, manip, link_name, return_all_solns=True) def nodecost(joints): robot = manip.GetRobot() saver = rave.Robot.RobotStateSaver(robot) robot.SetDOFValues(joints, manip.GetArmJoints()) return 100 * robot.GetEnv().CheckCollision(robot) paths, costs, timesteps = ku.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost) i_best = np.argmin(costs) print "lowest cost of initial trajs:", costs[i_best] if len(timesteps) < n_steps: print "timesteps with soln: ", timesteps print "linearly interpolating the rest" path_init = mu.interp2d(np.arange(n_steps), timesteps, paths[i_best]) else: print "all timesteps have an ik solution" path_init = paths[i_best] for i in xrange(1, n_steps - 1): print "waypoint xyz", xyzs[i] waypoint_cnt = { "type": "pose", "name": "waypoint_pose", "params": { "xyz": list(xyzs[i]), "wxyz": list(quats[i]), "link": link_name, # "pos_coeffs" : [10,10,10], # "rot_coeffs" : [10,10,10], "timestep": i, }, } request["constraints"].append(waypoint_cnt) request["init_info"]["type"] = "given_traj" request["init_info"]["data"] = [x.tolist() for x in path_init] return request