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
Example #2
0
 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
Example #3
0
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
Example #4
0
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
Example #5
0
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
Example #6
0
    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
Example #7
0
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
Example #8
0
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
Example #9
0
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
Example #10
0
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