pub fn forward_pose(&self, _cable_lengths: &Vec<f32>, _previous_pose: &Pose) -> Pose {
        const SGD_ROUNDS : usize = 32;
        let mut rate = 0.1;

        // set the initial condition of the gradient descent
        let mut pose = Vec::with_capacity(6);
        pose.push(_previous_pose.position[0] as f64);
        pose.push(_previous_pose.position[1] as f64);
        pose.push(_previous_pose.position[2] as f64);

        let axis = _previous_pose.orientation.scaled_axis();

        pose.push(1.0);
        pose.push(axis[0] as f64);
        pose.push(axis[1] as f64);
        pose.push(axis[2] as f64);

        for _ in 0..SGD_ROUNDS {
            // this function defines the error between the estimated and measured cable lengths
            // this function is implemented with an automatic differentiation type such that the
            // gradient can be calculated automatically later
            let error_func = |l : &[F]| {
                if let &[tx, ty, tz, qw, qx, qy, qz] = l {

                    let mag = (qw*qw + qx*qx + qy*qy + qz*qz).sqrt();
                    
                    let ux = qx / mag;
                    let uy = qy / mag;
                    let uz = qz / mag;


                    let mut loss = F::cst(0.0);
                    for (i,c) in self.cables.iter().enumerate() {
                        let err = c.calculate_pulley_state(tx, ty, tz, ux, uy, uz).0 - F::cst(_cable_lengths[i]);
                        loss += err * err;
                    }

                    loss
                } else {
                    F::cst(0.0)
                }
            };

            // we compute the gradient of the error...
            let g = grad(error_func, &pose);

            // ... then we tweek the estimated pose parameters (position and orientation)
            // in order to reduce the error from the measured 
            for i in 0..7 {
                pose[i] -= if i >= 3 {500.} else {1.} * rate * g[i];
            }

            rate *= 0.9;
        }
        let mut r = Pose::new();
        r.position = Vector3::new(pose[0] as f32, pose[1] as f32, pose[2] as f32);
        r.orientation = UnitQuaternion::new(Vector3::new(pose[4] as f32, pose[5] as f32, pose[6] as f32));

        r
    }
/// computes the pose (position and orientation) of the end effector given a measurement
/// of the cable lengths, as well as a previously know pose. The better an estimate the
/// previous pose is of the current pose, the better the resulting accuracy.

computes the pose (position and orientation) of the end effector given a measurement
of the cable lengths, as well as a previously know pose. The better an estimate the
previous pose is of the current pose, the better the resulting accuracy.
computes the pose ( position and orientation ) of the end effector given a measurement of the cable lengths as well as a previously know pose . The better an estimate the previous pose is of the current pose the better the resulting accuracy .
pub fn forward_pose(&self, _cable_lengths: &Vec<f32>, _previous_pose: &Pose) -> Pose {
        const SGD_ROUNDS : usize = 32;
        let mut rate = 0.1;
        // set the initial condition of the gradient descent
        let mut pose = Vec::with_capacity(6);
        pose.push(_previous_pose.position[0] as f64);
        pose.push(_previous_pose.position[1] as f64);
        pose.push(_previous_pose.position[2] as f64);
        let axis = _previous_pose.orientation.scaled_axis();
        pose.push(1.0);
        pose.push(axis[0] as f64);
        pose.push(axis[1] as f64);
        pose.push(axis[2] as f64);
        for _ in 0..SGD_ROUNDS {
            // this function defines the error between the estimated and measured cable lengths
            // this function is implemented with an automatic differentiation type such that the
            // gradient can be calculated automatically later
            let error_func = |l : &[F]| {
                if let &[tx, ty, tz, qw, qx, qy, qz] = l {
                    let mag = (qw*qw + qx*qx + qy*qy + qz*qz).sqrt();
                    let ux = qx / mag;
                    let uy = qy / mag;
                    let uz = qz / mag;
                    let mut loss = F::cst(0.0);
                    for (i,c) in self.cables.iter().enumerate() {
                        let err = c.calculate_pulley_state(tx, ty, tz, ux, uy, uz).0 - F::cst(_cable_lengths[i]);
                        loss += err * err;
                    }
                    loss
                } else {
                    F::cst(0.0)
                }
            };
            // we compute the gradient of the error...
            let g = grad(error_func, &pose);
            // ... then we tweek the estimated pose parameters (position and orientation)
            // in order to reduce the error from the measured 
            for i in 0..7 {
                pose[i] -= if i >= 3 {500.} else {1.} * rate * g[i];
            }
            rate *= 0.9;
        }
        let mut r = Pose::new();
        r.position = Vector3::new(pose[0] as f32, pose[1] as f32, pose[2] as f32);
        r.orientation = UnitQuaternion::new(Vector3::new(pose[4] as f32, pose[5] as f32, pose[6] as f32));
        r
    }
pub fn forward_pose ( & self , _cable_lengths : & Vec < f32 > , _previous_pose : & Pose ) -> Pose { const SGD_ROUNDS : usize = 32 ; let mut rate = 0.1 ; // set the initial condition of the gradient descent
 let mut pose = Vec :: with_capacity ( 6 ) ; pose . push ( _previous_pose . position [ 0 ] as f64 ) ; pose . push ( _previous_pose . position [ 1 ] as f64 ) ; pose . push ( _previous_pose . position [ 2 ] as f64 ) ; let axis = _previous_pose . orientation . scaled_axis ( ) ; pose . push ( 1.0 ) ; pose . push ( axis [ 0 ] as f64 ) ; pose . push ( axis [ 1 ] as f64 ) ; pose . push ( axis [ 2 ] as f64 ) ; for _ in 0 .. SGD_ROUNDS { // this function defines the error between the estimated and measured cable lengths
 // this function is implemented with an automatic differentiation type such that the
 // gradient can be calculated automatically later
 let error_func = | l : & [ F ] | { if let & [ tx , ty , tz , qw , qx , qy , qz ] = l { let mag = ( qw * qw + qx * qx + qy * qy + qz * qz ) . sqrt ( ) ; let ux = qx / mag ; let uy = qy / mag ; let uz = qz / mag ; let mut loss = F :: cst ( 0.0 ) ; for ( i , c ) in self . cables . iter ( ) . enumerate ( ) { let err = c . calculate_pulley_state ( tx , ty , tz , ux , uy , uz ) . 0 - F :: cst ( _cable_lengths [ i ] ) ; loss += err * err ; } loss } else { F :: cst ( 0.0 ) } } ; // we compute the gradient of the error...
 let g = grad ( error_func , & pose ) ; // ... then we tweek the estimated pose parameters (position and orientation)
 // in order to reduce the error from the measured 
 for i in 0 .. 7 { pose [ i ] -= if i >= 3 { 500. } else { 1. } * rate * g [ i ] ; } rate *= 0.9 ; } let mut r = Pose :: new ( ) ; r . position = Vector3 :: new ( pose [ 0 ] as f32 , pose [ 1 ] as f32 , pose [ 2 ] as f32 ) ; r . orientation = UnitQuaternion :: new ( Vector3 :: new ( pose [ 4 ] as f32 , pose [ 5 ] as f32 , pose [ 6 ] as f32 ) ) ; r }
computes the pose (position and orientation) of the end effector given a measurement
of the cable lengths, as well as a previously know pose.
computes the pose ( position and orientation ) of the end effector given a measurement of the cable lengths as well as a previously know pose .
&self
_cable_lengths
&Vec<f32>
_previous_pose
&Pose
&self
_cable_lengths
&Vec<f32>
_previous_pose
&Pose