optima 0.0.4

An easy to set up and easy optimization and planning toolbox, particularly for robotics.
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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151

use std::vec;
#[cfg(not(target_arch = "wasm32"))]
use pyo3::*;

#[cfg(target_arch = "wasm32")]
use wasm_bindgen::prelude::*;

use serde::{Serialize, Deserialize};
use crate::utils::utils_console::{optima_print, PrintColor, PrintMode};
use crate::utils::utils_robot::urdf_link::URDFLink;

/// A Link holds all necessary information about a robot link (specified by a robot URDF file)
/// in order to do kinematic and dynamic computations on a robot model.
#[cfg_attr(not(target_arch = "wasm32"), pyclass, derive(Clone, Debug, Serialize, Deserialize))]
#[cfg_attr(target_arch = "wasm32", wasm_bindgen, derive(Clone, Debug, Serialize, Deserialize))]
pub struct Link {
    name: String,
    present: bool,
    link_idx: usize,
    preceding_link_idx: Option<usize>,
    children_link_idxs: Vec<usize>,
    preceding_joint_idx: Option<usize>,
    is_chain_base_link: bool,
    urdf_link: URDFLink,
}
impl Link {
    pub fn new(urdf_link: URDFLink, link_idx: usize) -> Self {
        Self {
            name: urdf_link.name().to_string(),
            present: true,
            link_idx,
            preceding_link_idx: None,
            children_link_idxs: vec![],
            preceding_joint_idx: None,
            is_chain_base_link: false,
            urdf_link
        }
    }
    /// Returns a link that can serve as a base link.  This will be automatically used by the
    /// `RobotConfigurationModule`, so it will almost never need to be called by the end user.
    /// Here, the `child_link_idx` will be the link that the chain connects to.  For example,
    /// if the child_link_idx is the world_link_idx of the `RobotModelModule`, this will essentially
    /// create a mobile base link for the whole robot model.
    pub fn new_base_of_chain_link(link_idx: usize, child_link_idx: usize, _newly_created_joint_idx: usize, _world_link_idx: usize) -> Self {
        let name = format!("base_of_chain_link_with_child_link_{}", child_link_idx);
        Self {
            name,
            present: true,
            link_idx,
            preceding_link_idx: None ,
            children_link_idxs: vec![child_link_idx],
            preceding_joint_idx: None,
            is_chain_base_link: true,
            urdf_link: URDFLink::new_empty()
        }
    }
    pub fn name(&self) -> &str {
        &self.name
    }
    pub fn present(&self) -> bool {
        self.present
    }
    pub fn link_idx(&self) -> usize {
        self.link_idx
    }
    pub fn preceding_link_idx(&self) -> Option<usize> {
        self.preceding_link_idx
    }
    pub fn children_link_idxs(&self) -> &Vec<usize> {
        &self.children_link_idxs
    }
    pub fn preceding_joint_idx(&self) -> Option<usize> {
        self.preceding_joint_idx
    }
    pub fn is_chain_base_link(&self) -> bool {
        self.is_chain_base_link
    }
    pub fn urdf_link(&self) -> &URDFLink {
        &self.urdf_link
    }
    pub fn set_is_chain_base_link(&mut self, is_mobile_base_link: bool) {
        self.is_chain_base_link = is_mobile_base_link;
    }
    pub fn set_preceding_link_idx(&mut self, preceding_link_idx: Option<usize>) {
        self.preceding_link_idx = preceding_link_idx;
    }
    pub fn set_children_link_idxs(&mut self, children_link_idxs: Vec<usize>) {
        self.children_link_idxs = children_link_idxs;
    }
    pub fn set_preceding_joint_idx(&mut self, preceding_joint_idx: Option<usize>) {
        self.preceding_joint_idx = preceding_joint_idx;
    }
    pub fn add_child_link_idx(&mut self, idx: usize) {
        self.children_link_idxs.push(idx);
    }
    pub fn print_summary(&self) {
        optima_print(&format!("  Link index: "), PrintMode::Print, PrintColor::Blue, true, 0, None, vec![]);
        optima_print(&format!(" {} ", self.link_idx), PrintMode::Print, PrintColor::None, false, 0, None, vec![]);
        optima_print(&format!("  Link name: "), PrintMode::Print, PrintColor::Blue, true, 0, None, vec![]);
        optima_print(&format!(" {} ", self.name), PrintMode::Print, PrintColor::None, false, 0, None, vec![]);
        optima_print(&format!("  Present: "), PrintMode::Print, PrintColor::Blue, true, 0, None, vec![]);
        let color = match self.present {
            true => { PrintColor::Green }
            false => { PrintColor::Red }
        };
        optima_print(&format!(" {} ", self.present), PrintMode::Print, color, false, 0, None, vec![]);
        optima_print(&format!("  Preceding Joint: "), PrintMode::Print, PrintColor::Blue, true, 0, None, vec![]);
        optima_print(&format!(" {:?} ", self.preceding_joint_idx), PrintMode::Print, PrintColor::None, false, 0, None, vec![]);
        optima_print(&format!("  Preceding link idx: "), PrintMode::Print, PrintColor::Blue, true, 0, None, vec![]);
        optima_print(&format!(" {:?} ", self.preceding_link_idx()), PrintMode::Print, PrintColor::None, false, 0, None, vec![]);
        optima_print(&format!("  Children link idxs: "), PrintMode::Print, PrintColor::Blue, true, 0, None, vec![]);
        optima_print(&format!(" {:?} ", self.children_link_idxs), PrintMode::Print, PrintColor::None, false, 0, None, vec![]);
    }
    pub fn set_present(&mut self, present: bool) {
        self.present = present;
    }
}

/// Methods supported by python.
#[cfg(not(target_arch = "wasm32"))]
#[pymethods]
impl Link {
    pub fn name_py(&self) -> String {
        self.name.clone()
    }
    pub fn present_py(&self) -> bool {
        self.present
    }
    pub fn link_idx_py(&self) -> usize {
        self.link_idx
    }
    pub fn preceding_link_idx_py(&self) -> Option<usize> {
        return self.preceding_link_idx
    }
    pub fn children_link_idxs_py(&self) -> Vec<usize> {
        self.children_link_idxs.clone()
    }
    pub fn preceding_joint_idx_py(&self) -> Option<usize> {
        self.preceding_joint_idx
    }
    pub fn is_chain_base_link_py(&self) -> bool {
        self.is_chain_base_link
    }
}

/// Methods supported by WASM.
#[cfg(target_arch = "wasm32")]
#[wasm_bindgen]
impl Link {

}