def generate_plan(self, behavior):
        plan = drc.footstep_plan_t()
        plan.utime = 0
        plan.params = drc.footstep_plan_params_t()
        plan.params.behavior = behavior
        plan.footsteps = []
        plan.num_steps = 10

        for j in range(plan.num_steps):
            goal = drc.footstep_t()
            goal.utime = 0
            goal.pos = drc.position_3d_t()
            goal.pos.translation = drc.vector_3d_t()
            goal.params = drc.footstep_params_t()
            if 1 <= j <= 2:  # this works in Python!
                goal.pos.translation.x = 0
                goal.pos.translation.y = 0
                goal.pos.translation.z = 0
                goal.pos.rotation = drc.quaternion_t()
                goal.pos.rotation.x = 0
                goal.pos.rotation.y = 0
                goal.pos.rotation.z = 0
                goal.pos.rotation.w = 1
            else:
                goal.pos.translation.x = 0.15 * j + (0.5 -
                                                     random.random()) * 0.15
                goal.pos.translation.y = random.random() * 0.15 * j + (
                    0.5 - random.random()) * 0.2
                goal.pos.translation.z = (0.5 - random.random()) * 0.2
                goal.pos.rotation = drc.quaternion_t()
                goal.pos.rotation.x = random.random()
                goal.pos.rotation.y = random.random()
                goal.pos.rotation.z = random.random()
                goal.pos.rotation.w = random.random()
            goal.id = j + 1
            goal.is_right_foot = j % 2 == 0
            goal.is_in_contact = True
            goal.fixed_x = True
            goal.fixed_y = True
            goal.fixed_z = True
            goal.fixed_roll = True
            goal.fixed_pitch = True
            goal.fixed_yaw = True
            goal.params.step_speed = 1.5
            goal.params.step_height = random.random() * 0.25
            goal.params.bdi_step_duration = 2.0
            goal.params.bdi_sway_duration = 0
            goal.params.bdi_lift_height = random.random() * 0.15
            goal.params.bdi_toe_off = 0
            goal.params.bdi_knee_nominal = 0
            goal.num_terrain_pts = 3
            goal.terrain_path_dist = [0, 0.1, 0.2]
            goal.terrain_height = [0, random.random() * 0.15, 0]

            plan.footsteps.append(goal)
        return plan
Exemple #2
0
def encode_footstep_plan(footsteps, params):
    plan = drc.footstep_plan_t()
    plan.num_steps = len(footsteps)
    plan.footsteps = [step.to_footstep_t() for step in footsteps]
    if params is not None:
        plan.params = params
    else:
        plan.params = drc.footstep_plan_params_t()
    plan.iris_region_assignments = [-1 for f in footsteps]
    return plan
Exemple #3
0
def encode_footstep_plan(footsteps, params):
    plan = drc.footstep_plan_t()
    plan.num_steps = len(footsteps)
    plan.footsteps = [step.to_footstep_t() for step in footsteps]
    if params is not None:
        plan.params = params
    else:
        plan.params = drc.footstep_plan_params_t()
    plan.iris_region_assignments = [-1 for f in footsteps]
    return plan
    def generate_plan(self, behavior):
        plan = drc.footstep_plan_t()
        plan.utime = 0
        plan.params = drc.footstep_plan_params_t()
        plan.params.behavior = behavior
        plan.footsteps = []
        plan.num_steps = 10

        for j in range(plan.num_steps):
            goal = drc.footstep_t()
            goal.utime = 0
            goal.pos = drc.position_3d_t();
            goal.pos.translation = drc.vector_3d_t()
            goal.params = drc.footstep_params_t()
            if 1 <= j <= 2:  # this works in Python!
                goal.pos.translation.x = 0
                goal.pos.translation.y = 0
                goal.pos.translation.z = 0
                goal.pos.rotation = drc.quaternion_t()
                goal.pos.rotation.x = 0
                goal.pos.rotation.y = 0
                goal.pos.rotation.z = 0
                goal.pos.rotation.w = 1
            else:
                goal.pos.translation.x = 0.15 * j + (0.5 - random.random()) * 0.15
                goal.pos.translation.y = random.random() * 0.15 * j + (0.5 - random.random()) * 0.2
                goal.pos.translation.z = (0.5 - random.random()) * 0.2
                goal.pos.rotation = drc.quaternion_t()
                goal.pos.rotation.x = random.random()
                goal.pos.rotation.y = random.random()
                goal.pos.rotation.z = random.random()
                goal.pos.rotation.w = random.random()
            goal.id = j+1
            goal.is_right_foot = j % 2 == 0
            goal.is_in_contact = True
            goal.fixed_x = True
            goal.fixed_y = True
            goal.fixed_z = True
            goal.fixed_roll = True
            goal.fixed_pitch= True
            goal.fixed_yaw = True
            goal.params.step_speed = 1.5
            goal.params.step_height = random.random() * 0.25
            goal.params.bdi_step_duration = 2.0
            goal.params.bdi_sway_duration = 0
            goal.params.bdi_lift_height = random.random() * 0.15
            goal.params.bdi_toe_off = 0
            goal.params.bdi_knee_nominal = 0
            goal.num_terrain_pts = 3
            goal.terrain_path_dist = [0, 0.1, 0.2]
            goal.terrain_height = [0, random.random() * 0.15, 0]

            plan.footsteps.append(goal)
        return plan
Exemple #5
0
    def request_handler(channel, data):
        request = drc.footstep_plan_request_t.decode(data)
        # ----- Send LCM message based on received info -----
        # Create Footstep planner and get solution
        f = FootstepPlanner(2)

        # Create Atlas object
        print("----------------------------------")
        pm = PackageMap()
        model = os.path.abspath(
            os.path.join(os.sep, "home", "rahuly", "Data", "drake-new",
                         "drake-distro", "drake", "examples", "atlas", "urdf",
                         "atlas_minimal_contact.urdf"))
        pm.PopulateUpstreamToDrake(model)
        robot = rbtree.RigidBodyTree(
            model,
            package_map=pm,
            floating_base_type=rbtree.FloatingBaseType.kQuaternion)
        t = request.initial_state.pose.translation
        r = request.initial_state.pose.rotation
        q = [t.x, t.y, t.z] + [r.w, r.x, r.y, r.z] + list(
            request.initial_state.joint_position)
        q = tuple(q)
        v = [0] * 6 + list(request.initial_state.joint_velocity)
        v = tuple(v)
        kinsol = robot.doKinematics(q, v)
        r_startpos = robot.transformPoints(
            kinsol, np.zeros((3, 1)),
            robot.findFrame("r_foot_sole").get_frame_index(), 0)
        l_startpos = robot.transformPoints(
            kinsol, np.zeros((3, 1)),
            robot.findFrame("l_foot_sole").get_frame_index(), 0)

        print("r_startpos: " + str(r_startpos))
        print("l_startpos: " + str(l_startpos))
        print("----------------------------------")

        f.setStartRL(r_startpos, l_startpos)
        f.setGoal(
            [request.goal_pos.translation.x, request.goal_pos.translation.y])
        f.setReachable([(0, -0.2), (0.5, 0), (0, 0.2), (-0.3, 0)], 0.3)
        # f.setNominal(0.5)
        f.solveProgram()

        # Package solution into footstep_plan_t
        plan = drc.footstep_plan_t()

        # - Footsteps
        plan.num_steps = 2 * f.numFootsteps
        for fNum in range(0, f.numFootsteps):
            # -- Right Footstep
            rstep = drc.footstep_t()
            rstep.pos.rotation.w = 1
            rspos = np.array([[f.footsteps[2 * fNum][0]],
                              [f.footsteps[2 * fNum][1]], [0]])
            # rs = np.array([f.footsteps[2*fNum][0], f.footsteps[2*fNum][1], 0])
            rs = robot.transformPoints(
                kinsol, rspos,
                robot.FindBody("r_foot").get_body_index(),
                robot.findFrame("r_foot_sole").get_frame_index())
            print("Right: " + str(rs))
            rstep.pos.translation.x = rs[0]
            rstep.pos.translation.y = rs[1]
            rstep.pos.translation.z = rs[2]
            # pos.rotation ??
            rstep.id = 2 * fNum
            rstep.is_right_foot = True
            rstep.params = request.default_step_params
            plan.footsteps.append(rstep)
            # -- Left Footstep
            lstep = drc.footstep_t()
            lstep.pos.rotation.w = 1
            lspos = np.array([[f.footsteps[2 * fNum + 1][0]],
                              [f.footsteps[2 * fNum + 1][1]], [0]])
            # ls = np.array([f.footsteps[2*fNum+1][0], f.footsteps[2*fNum+1][1], 0])
            ls = robot.transformPoints(
                kinsol, lspos,
                robot.FindBody("l_foot").get_body_index(),
                robot.findFrame("l_foot_sole").get_frame_index())
            print("Left: " + str(ls))
            lstep.pos.translation.x = ls[0]
            lstep.pos.translation.y = ls[1]
            lstep.pos.translation.z = ls[2]
            # pos.rotation ??
            lstep.id = 2 * fNum + 1
            lstep.is_right_foot = False
            lstep.params = request.default_step_params
            plan.footsteps.append(lstep)

        # - IRIS Regions
        plan.num_iris_regions = request.num_iris_regions
        plan.iris_regions = request.iris_regions
        plan.iris_region_assignments = [-1] * plan.num_steps

        # - Params: AFTER WE HAVE REQUESTS WORKING
        plan.params = request.params

        # Send footstep_plan_t
        lc.publish("FOOTSTEP_PLAN", plan.encode())