copp 0.2.0

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

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

use crate::copp::copp3::opt3::topp3_socp::{
    topp3_socp as rust_topp3_socp, topp3_socp_expert_with_info as rust_topp3_socp_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-SOCP 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`.
///
/// # 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_socp(
    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_socp(&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-SOCP problem and always return Clarabel diagnostics.
///
/// This expert entry mirrors `topp3_socp_expert`: 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-SOCP 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.
///
/// # Safety
/// Same safety requirements as `topp3_socp`. `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_socp_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_socp_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(),
    }
}