Struct openrr_planner::JacobianIkSolver

pub struct JacobianIkSolver<T>
where
    T: RealField,
{
    pub allowable_target_distance: T,
    pub allowable_target_angle: T,
    pub jacobian_multiplier: T,
    pub num_max_try: usize,
    /* private fields */
}

Inverse Kinematics Solver using Jacobian matrix

Fields

allowable_target_distance: T

If the distance is smaller than this value, it is reached.

allowable_target_angle: T

If the angle distance is smaller than this value, it is reached.

jacobian_multiplier: T

multiplier for jacobian

num_max_try: usize

How many times the joints are tried to be moved

Implementations

impl<T> JacobianIkSolver<T>

where T: RealField + SubsetOf<f64>,

pub fn new( allowable_target_distance: T, allowable_target_angle: T, jacobian_multiplier: T, num_max_try: usize, ) -> JacobianIkSolver<T>

Create instance of JacobianIkSolver.

JacobianIkSolverBuilder is available instead of calling this new method.

Examples

let solver = k::JacobianIkSolver::new(0.01, 0.01, 0.5, 100);

pub fn set_nullspace_function( &mut self, func: Box<dyn Fn(&[T]) -> Vec<T> + Sync + Send>, )

Set a null space function for redundant manipulator.

Examples

let mut solver = k::JacobianIkSolver::new(0.01, 0.01, 0.5, 100);
solver.set_nullspace_function(Box::new(
k::create_reference_positions_nullspace_function(
   vec![0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
   vec![0.1, 0.1, 0.1, 1.0, 0.1, 0.5, 0.0],
   ),
));

pub fn clear_nullspace_function(&mut self)

Clear the null function which is set by set_nullspace_function.

Trait Implementations

impl<T> Debug for JacobianIkSolver<T>

where T: RealField + Debug,

fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), Error>

Formats the value using the given formatter.

impl<T> Default for JacobianIkSolver<T>

where T: RealField + SubsetOf<f64>,

fn default() -> JacobianIkSolver<T>

Returns the “default value” for a type.

impl<T> InverseKinematicsSolver<T> for JacobianIkSolver<T>

where T: RealField + SubsetOf<f64>,

fn solve( &self, arm: &SerialChain<T>, target_pose: &Isometry<T, Unit<Quaternion<T>>, 3>, ) -> Result<(), Error>

Set joint positions of arm to reach the target_pose

Examples

use k::prelude::*;

let chain = k::Chain::<f32>::from_urdf_file("urdf/sample.urdf").unwrap();
// Create sub-`Chain` to make it easy to use inverse kinematics
let target_joint_name = "r_wrist_pitch";
let r_wrist = chain.find(target_joint_name).unwrap();
let mut arm = k::SerialChain::from_end(r_wrist);
println!("arm: {arm}");

// Set joint positions of `arm`
let positions = vec![0.1, 0.2, 0.0, -0.5, 0.0, -0.3];
arm.set_joint_positions(&positions).unwrap();
println!("initial positions={:?}", arm.joint_positions());

// Get the transform of the end of the manipulator (forward kinematics)
let mut target = arm.update_transforms().last().unwrap().clone();

println!("initial target pos = {}", target.translation);
println!("move x: -0.1");
target.translation.vector.x -= 0.1;

// Create IK solver with default settings
let solver = k::JacobianIkSolver::default();

// solve and move the manipulator positions
solver.solve(&arm, &target).unwrap();
println!("solved positions={:?}", arm.joint_positions());

fn solve_with_constraints( &self, arm: &SerialChain<T>, target_pose: &Isometry<T, Unit<Quaternion<T>>, 3>, constraints: &Constraints, ) -> Result<(), Error>

Set joint positions of arm to reach the target_pose with constraints

If you want to loose the constraints, use this method. For example, ignoring rotation with an axis. It enables to use the arms which has less than six DoF.

Example

use k::prelude::*;

let chain = k::Chain::<f32>::from_urdf_file("urdf/sample.urdf").unwrap();
let target_joint_name = "r_wrist_pitch";
let r_wrist = chain.find(target_joint_name).unwrap();
let mut arm = k::SerialChain::from_end(r_wrist);
let positions = vec![0.1, 0.2, 0.0, -0.5, 0.0, -0.3];
arm.set_joint_positions(&positions).unwrap();
let mut target = arm.update_transforms().last().unwrap().clone();
target.translation.vector.x -= 0.1;
let solver = k::JacobianIkSolver::default();

let mut constraints = k::Constraints::default();
constraints.rotation_x = false;
constraints.rotation_z = false;
solver
   .solve_with_constraints(&arm, &target, &constraints)
   .unwrap_or_else(|err| {
       println!("Err: {err}");
   });