Point-to-Point Motion Planning

PointToPointPathService.Plan (client.point_to_point_path) plans a collision-free motion through a sequence of goals and saves the result as a config of type CONFIG_TYPE_P2P_PATH. Read it back with ConfigService.Get.

Note

PointToPointPathService requires the point_to_point_path license (Activate Licenses).

Request

`PointToPointPathRequest`
Field	Description
`path_name`	Required. Name under which the planned path is saved.
`collision_config_name`	Required. A saved collision configuration (Collision Configurations).
`joint_waypoints`	One of. `PTPJointWaypoints`: `system_name`, `tcp_name` (used to compute Cartesian poses) and a list of `JointWaypoint`.
`target_waypoints`	One of. `PTPTargetWaypoints`: names of saved system poses (`TargetWaypoint` configs).

Result

point_to_point_path.Path (output only):

header: the path name;
system_name, tcp_name, collision_config_name;
cartesian_waypoints: TCP poses of the goals (always present);
joint_waypoints: the planned joint trajectory (present when planning succeeded);
tcp_to_attach_object, object_grasp_pose: used when carrying an object.

Planning between joint configurations

From examples/python/point_to_point_path_example.py:

print("Running point-to-point path planning queries...")
def run_plan(path_name, system_name, tcp_name, collision_config_name, joint_waypoints):
    req = PointToPointPathRequest(
        path_name=path_name,
        collision_config_name=collision_config_name,
        joint_waypoints=PTPJointWaypoints(
            system_name=system_name,
            tcp_name=tcp_name,
            joint_waypoints=joint_waypoints,
        ),
    )
    try:
        resp = client.point_to_point_path.stub.Plan(req)
        print("")
        if (resp.status.code == 0):
            print(f"Plan successful")

            get_resp = client.config.stub.Get(
                TypeNameConfigRequest(name=path_name, type=CONFIG_TYPE_P2P_PATH)
            )
            if get_resp.status.code == 0 and get_resp.HasField("p2p_path"):
                path = get_resp.p2p_path
                print(f"  Cartesian waypoints ({len(path.cartesian_waypoints)}):")
                for i, wp in enumerate(path.cartesian_waypoints):
                    p = wp.pose.position
                    q = wp.pose.rotation
                    print(f"    [{i}] pos=({p.x:.4f}, {p.y:.4f}, {p.z:.4f})  "
                          f"rot=({q.x:.4f}, {q.y:.4f}, {q.z:.4f}, {q.w:.4f})")
                print(f"  Joint waypoints ({len(path.joint_waypoints)}):")
                for i, wp in enumerate(path.joint_waypoints):
                    joints = ", ".join(f"{v:.4f}" for v in wp.position)
                    print(f"    [{i}] [{joints}]")
            else:
                print(f"  Failed to retrieve path: {get_resp.status.message}")


        else:
            print(f"Plan failed with status code {resp.status.code}: {resp.status.message}")
    except grpc.RpcError as e:
        print(f"Plan failed: {e}")

from flexiv.pdk.spatial.waypoint_pb2 import JointWaypoint
import math
kDegToRad = math.pi / 180.0

# 1. No collision
run_plan(
    "ptp_path_1", "system_1", "system_1.arm.flange", "default_collision_config",
    [
        JointWaypoint(position=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
        JointWaypoint(position=[0.0, -40.0 * kDegToRad, 0.0, 90.0 * kDegToRad, 0.0, 40.0 * kDegToRad, 0.0])
    ]
)

The example prints grpc.RpcError instead of raising it. Production code should check both layers; see Error Handling.

Attach an object to the flange (by parenting it to system_1.arm.flange) and it moves with the robot and is collision-checked along the path. The example does this with box_4.

Planning between saved system poses

A system pose (scene.TargetWaypoint) stores a TCP name together with a Cartesian pose and the joint configuration that reaches it. Save poses once, then plan between them by name. From examples/python/system_pose_example.py:

print("Saving system pose object to the scene...")
system_pose_object = TargetWaypoint(
    header=make_header("system_pose_1", parent="system_1"),
    tcp_name="system_1.arm.flange",
    cartesian=CartesianWaypoint(
        pose=fk_pose_1,
    ),
    joint=JointWaypoint(
        position=joint_configuration_1
    )
)

try:
    client.config.stub.Save(SaveConfigRequest(system_pose=system_pose_object))
except grpc.RpcError as e:
    print(f"Save system pose object failed: {e}")

print("Running point-to-point path plan using the system pose target waypoints...")
try:
    plan_response = client.point_to_point_path.stub.Plan(PointToPointPathRequest(
        path_name="ptp_path_system_pose_1",
        collision_config_name="default_collision_config",
        target_waypoints=PTPTargetWaypoints(
            target_waypoint_names=["system_pose_1", "system_pose_2"]
        )
    ))
    if plan_response.status.code == 0:
        print("Point-to-point path plan successful")
    else:
        print(f"Point-to-point path plan failed: {plan_response.status.message}")
except grpc.RpcError as e:
    print(f"Point-to-point path plan failed: {e}")

C++

examples/cpp/src/point_to_point_path_example.cpp does the same in C++ (kDegToRad is defined in the example as π / 180):

std::cout << "Running point-to-point path planning queries..." << std::endl;
const std::vector<double> home = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
const std::vector<double> clear_goal
    = {0.0, -40.0 * kDegToRad, 0.0, 90.0 * kDegToRad, 0.0, 40.0 * kDegToRad, 0.0};
const std::vector<double> box_1_goal = {13.24 * kDegToRad, -69.97 * kDegToRad, 0.06 * kDegToRad,
    112.91 * kDegToRad, 1.03 * kDegToRad, 92.89 * kDegToRad, 12.23 * kDegToRad};

// 1. No collision
RunPlan(client, "ptp_path_1", "system_1", "system_1.arm.flange", "default_collision_config",
    {home, clear_goal});
// 2. With collision (between system_1.arm and box_1)
RunPlan(client, "ptp_path_2", "system_1", "system_1.arm.flange", "default_collision_config",
    {home, box_1_goal});
// 3. With collision (between box_4 and box_1)
RunPlan(client, "ptp_path_3", "system_1", "system_1.arm.flange", "collision_config_without_box_1",
    {home, box_1_goal});
// 4. No collision
RunPlan(client, "ptp_path_4", "system_1", "system_1.arm.flange",
    "collision_config_without_box_1_and_box_4", {home, box_1_goal});

RunPlan fills the joint waypoints, calls Plan and reads the saved path back:

void RunPlan(Client& client, const std::string& path_name, const std::string& system_name,
    const std::string& tcp_name, const std::string& collision_config_name,
    const std::vector<std::vector<double>>& joint_waypoints)
{
  flexiv::pdk::point_to_point_path::PointToPointPathRequest request;
  request.set_path_name(path_name);
  request.set_collision_config_name(collision_config_name);
  auto* waypoints = request.mutable_joint_waypoints();
  waypoints->set_system_name(system_name);
  waypoints->set_tcp_name(tcp_name);
  for (const auto& positions : joint_waypoints) {
    auto* waypoint = waypoints->add_joint_waypoints();
    for (double position : positions) {
      waypoint->add_position(position);
    }
  }

  grpc::ClientContext context;
  flexiv::pdk::point_to_point_path::PointToPointPathResponse response;
  const grpc::Status status = client.point_to_point_path().stub->Plan(&context, request, &response);
  if (!status.ok()) {
    std::cerr << "Plan failed: " << status.error_message() << std::endl;
    return;
  }
  std::cout << std::endl;
  if (response.status().code() == Status::OK) {
    std::cout << "Plan successful" << std::endl;
    PrintPath(client, path_name);
  } else {
    std::cout << "Plan failed with status code " << response.status().code() << ": "
              << response.status().message() << std::endl;
  }
}

The ROS 2 package in examples/ros builds the same source:

PDK_SERVER_ADDR=192.168.1.100:50052 ros2 run pdk_cpp_examples point_to_point_path_example