copp 0.2.2

Convex-objective path parameterization for robotic trajectory planning.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132

//! C ABI wrappers for TOPP3-LP solvers.

use crate::copp::copp3::opt3::topp3_lp::{
    topp3_lp as rust_topp3_lp, topp3_lp_expert_with_info as rust_topp3_lp_expert,
};
use crate::ffi::c::core::status::panic_to_status;
use crate::ffi::c::solver::copp3::socp::Copp3SocpResult;
use crate::ffi::c::{CoppClarabelOptions, CoppProfile3rd, CoppStatus, Topp3Problem};
use std::panic::{AssertUnwindSafe, catch_unwind};

/// Solve a TOPP3-LP problem.
///
/// This C ABI builds an internal TOPP3 problem from the borrowed `Topp3Problem`,
/// forwards `options` to the Clarabel backend, and returns only the accepted
/// third-order profile.
///
/// On success, `out_profile` owns COPP-allocated `a` and `b` vectors and must
/// be released with `copp_profile_3rd_free`.
///
/// # Example
/// The example below solves TOPP3-LP and releases the accepted third-order
/// profile.
///
/// ```c
/// struct CoppClarabelOptions options;
/// struct CoppProfile3rd profile = {{0}, {0}, 0, 0};
///
/// check(copp_clarabel_default_options(&options));
/// check(topp3_lp(problem, options, &profile));
/// copp_profile_3rd_free(profile);
/// ```
///
/// # Safety
/// `problem.robot` must be a non-null mutable handle returned by
/// `copp_robot_create` and must remain valid for the duration of this call.
/// The solver builds the internal problem by linearizing third-order constraints,
/// which mutates the robot's internal constraint cache. `out_profile` must be
/// valid for one `CoppProfile3rd` write.
#[unsafe(no_mangle)]
pub unsafe extern "C" fn topp3_lp(
    problem: Topp3Problem,
    options: CoppClarabelOptions,
    out_profile: *mut CoppProfile3rd,
) -> CoppStatus {
    crate::ffi::c::core::status::clear_last_error();
    if out_profile.is_null() {
        return CoppStatus::NullPointer.into_ffi_status();
    }
    // SAFETY: `out_profile` was checked for null above and is expected to be
    // valid for one write by the C ABI contract.
    unsafe {
        out_profile.write(CoppProfile3rd::empty());
    }

    match catch_unwind(AssertUnwindSafe(|| {
        let options = options.build();
        // SAFETY: The C ABI contract requires the borrowed robot and
        // linearization slice inside `problem` to be valid for this call.
        let problem = unsafe { problem.build_rust()? };
        let profile =
            rust_topp3_lp(&problem, &options).map_err(|error| CoppStatus::from(&error))?;

        // SAFETY: Same checked output location as above.
        unsafe {
            out_profile.write(CoppProfile3rd::from_parts(
                profile.a,
                profile.b,
                profile.num_stationary,
            ));
        }
        Ok(CoppStatus::Ok)
    })) {
        Ok(Ok(status)) | Ok(Err(status)) => status.into_ffi_status(),
        Err(payload) => panic_to_status(payload).into_ffi_status(),
    }
}

/// Solve a TOPP3-LP problem and always return Clarabel diagnostics.
///
/// This expert entry follows the advanced-diagnostics convention: true
/// runtime failures still return a non-OK `CoppStatus`, but non-accepted
/// Clarabel statuses are reported inside `out_result` with
/// `has_profile == false` and the function returns `COPP_STATUS_OK`.
///
/// TOPP3-LP has no user-provided COPP objective list, so
/// `out_result.objective_value` is NaN and `out_result.objective_terms` is an
/// empty vector. Raw Clarabel objective values and solver diagnostics are still
/// populated.
///
/// Use the same advanced-diagnostics pattern as \ref copp2_socp_expert. The
/// result type is `Copp3SocpResult`, and `has_profile` replaces `has_a`.
///
/// # Safety
/// Same safety requirements as `topp3_lp`. `out_result` must be valid for one
/// `Copp3SocpResult` write and must later be released with
/// `copp3_socp_result_free`.
#[unsafe(no_mangle)]
pub unsafe extern "C" fn topp3_lp_expert(
    problem: Topp3Problem,
    options: CoppClarabelOptions,
    out_result: *mut Copp3SocpResult,
) -> CoppStatus {
    crate::ffi::c::core::status::clear_last_error();
    if out_result.is_null() {
        return CoppStatus::NullPointer.into_ffi_status();
    }
    // SAFETY: `out_result` was checked for null above and is expected to be
    // valid for one write by the C ABI contract.
    unsafe {
        out_result.write(Copp3SocpResult::empty());
    }

    match catch_unwind(AssertUnwindSafe(|| {
        let options = options.build();
        // SAFETY: The C ABI contract requires the borrowed robot and
        // linearization slice inside `problem` to be valid for this call.
        let problem = unsafe { problem.build_rust()? };
        let expert =
            rust_topp3_lp_expert(&problem, &options).map_err(|error| CoppStatus::from(&error))?;
        let result =
            Copp3SocpResult::from_solution(expert.result, expert.solution, expert.linsolver, None);

        // SAFETY: Same checked output location as above.
        unsafe {
            out_result.write(result);
        }
        Ok(CoppStatus::Ok)
    })) {
        Ok(Ok(status)) | Ok(Err(status)) => status.into_ffi_status(),
        Err(payload) => panic_to_status(payload).into_ffi_status(),
    }
}