rs_opw_kinematics::utils

Function joints

Source 
pub fn joints(angles: &[f32; 6]) -> Joints
Expand description

Convert array of f32’s in degrees to Joints that are array of f64’s in radians

Examples found in repository?
examples/cartesian_stroke.rs (line 106)
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
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
fn main() {
    fn pose(kinematics: &KinematicsWithShape, angles_in_degrees: [f32; 6]) -> Pose {
        kinematics.forward(&utils::joints(&angles_in_degrees))
    }
    
    // Initialize kinematics with your robot's specific parameters
    let k = create_rx160_robot();

    // Starting point, where the robot exists at the beginning of the task.
    let start = utils::joints(&[-120.0, -90.0, -92.51, 18.42, 82.23, 189.35]);

    // In production, other poses are normally given in Cartesian, but here they are given
    // in joints as this way it is easier to craft when in rs-opw-kinematics IDE.

    // "Landing" pose close to the surface, from where Cartesian landing on the surface
    // is possible and easy. Robot will change into one of the possible alternative configurations 
    // between start and land.
    let land = pose(&k, [-120.0, -10.0, -92.51, 18.42, 82.23, 189.35]);

    let steps: Vec<Pose> = [
        pose(&k, [-225.0, -27.61, 88.35, -85.42, 44.61, 138.0]),
        pose(&k, [-225.0, -33.02, 134.48, -121.08, 54.82, 191.01]),
        //pose(&k, [-225.0, 57.23, 21.61, -109.48, 97.50, 148.38]) // this collides
    ]
    .into();

    // "Parking" pose, Cartesian lifting from the surface at the end of the stroke. Park where we landed.
    let park = pose(&k, [-225.0, -27.61, 88.35, -85.42, 44.61, 110.0]);

    // Creat Cartesian planner
    let planner = Cartesian {
        robot: &k, // The robot, instance of KinematicsWithShape
        check_step_m: 0.02, // Pose distance check accuracy in meters (for translation)
        check_step_rad: 3.0_f64.to_radians(), // Pose distance check accuracy in radians (for rotation)
        max_transition_cost: 3_f64.to_radians(), // Maximal transition costs (not tied to the parameter above)
        // (weighted sum of abs differences between 'from' and 'to' for all joints, radians).
        transition_coefficients: DEFAULT_TRANSITION_COSTS, // Joint weights to compute transition cost
        linear_recursion_depth: 8,

        // RRT planner that computes the non-Cartesian path from starting position to landing pose
        rrt: RRTPlanner {
            step_size_joint_space: 2.0_f64.to_radians(), // RRT planner step in joint space
            max_try: 1000,
            debug: true,
        },
        include_linear_interpolation: true, // If true, intermediate Cartesian poses are
        // included in the output. Otherwise, they are checked but not included in the output
        debug: true,
    };

    // plan path
    let started = Instant::now();
    let path = planner.plan(&start, &land, steps, &park);
    let elapsed = started.elapsed();

    match path {
        Ok(path) => {
            for joints in path {
                println!("{:?}", &joints);
            }
        }
        Err(message) => {
            println!("Failed: {}", message);
        }
    }
    println!("Took {:?}", elapsed);
}
More examples
Hide additional examples
examples/path_planning_rrt.rs (line 137)
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
fn main() {
    // Initialize kinematics with your robot's specific parameters
    let kinematics = create_rx160_robot();

    // This is pretty tough path that requires to lift the initially low placed
    // tool over the obstacle and then lower again. Direct path is interrupted
    // by obstacle.
    println!("** Tough example **");
    let start = utils::joints(&[-120.0, -90.0, -92.51, 18.42, 82.23, 189.35]);
    let goal = utils::joints(&[40.0, -90.0, -92.51, 18.42, 82.23, 189.35]);
    example(start, goal, &kinematics);

    // Simple short step
    println!("** Simple example **");
    let start = utils::joints(&[-120.0, -90.0, -92.51, 18.42, 82.23, 189.35]);
    let goal = utils::joints(&[-120.0, -80.0, -90., 18.42, 82.23, 189.35]);
    example(start, goal, &kinematics);
}