Struct k::JacobianIkSolver

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

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§

source§

impl<T> JacobianIkSolver<T>
where T: RealField + SubsetOf<f64>,

source

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);
source

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

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],
   ),
));
source

pub fn clear_nullspace_function(&mut self)

Clear the null function which is set by set_nullspace_function.

Trait Implementations§

source§

impl<T: RealField + Debug> Debug for JacobianIkSolver<T>

source§

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

Formats the value using the given formatter. Read more
source§

impl<T> Default for JacobianIkSolver<T>
where T: RealField + SubsetOf<f64>,

source§

fn default() -> Self

Returns the “default value” for a type. Read more
source§

impl<T> InverseKinematicsSolver<T> for JacobianIkSolver<T>
where T: RealField + SubsetOf<f64>,

source§

fn solve( &self, arm: &SerialChain<T>, target_pose: &Isometry3<T>, ) -> 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());
source§

fn solve_with_constraints( &self, arm: &SerialChain<T>, target_pose: &Isometry3<T>, 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}");
   });

Auto Trait Implementations§

§

impl<T> Freeze for JacobianIkSolver<T>
where T: Freeze,

§

impl<T> !RefUnwindSafe for JacobianIkSolver<T>

§

impl<T> Send for JacobianIkSolver<T>

§

impl<T> Sync for JacobianIkSolver<T>

§

impl<T> Unpin for JacobianIkSolver<T>
where T: Unpin,

§

impl<T> !UnwindSafe for JacobianIkSolver<T>

Blanket Implementations§

source§

impl<T> Any for T
where T: 'static + ?Sized,

source§

fn type_id(&self) -> TypeId

Gets the TypeId of self. Read more
source§

impl<T> Borrow<T> for T
where T: ?Sized,

source§

fn borrow(&self) -> &T

Immutably borrows from an owned value. Read more
source§

impl<T> BorrowMut<T> for T
where T: ?Sized,

source§

fn borrow_mut(&mut self) -> &mut T

Mutably borrows from an owned value. Read more
source§

impl<T> From<T> for T

source§

fn from(t: T) -> T

Returns the argument unchanged.

source§

impl<T> Instrument for T

source§

fn instrument(self, span: Span) -> Instrumented<Self>

Instruments this type with the provided Span, returning an Instrumented wrapper. Read more
source§

fn in_current_span(self) -> Instrumented<Self>

Instruments this type with the current Span, returning an Instrumented wrapper. Read more
source§

impl<T, U> Into<U> for T
where U: From<T>,

source§

fn into(self) -> U

Calls U::from(self).

That is, this conversion is whatever the implementation of From<T> for U chooses to do.

source§

impl<T> Same for T

source§

type Output = T

Should always be Self
source§

impl<SS, SP> SupersetOf<SS> for SP
where SS: SubsetOf<SP>,

source§

fn to_subset(&self) -> Option<SS>

The inverse inclusion map: attempts to construct self from the equivalent element of its superset. Read more
source§

fn is_in_subset(&self) -> bool

Checks if self is actually part of its subset T (and can be converted to it).
source§

fn to_subset_unchecked(&self) -> SS

Use with care! Same as self.to_subset but without any property checks. Always succeeds.
source§

fn from_subset(element: &SS) -> SP

The inclusion map: converts self to the equivalent element of its superset.
source§

impl<T, U> TryFrom<U> for T
where U: Into<T>,

source§

type Error = Infallible

The type returned in the event of a conversion error.
source§

fn try_from(value: U) -> Result<T, <T as TryFrom<U>>::Error>

Performs the conversion.
source§

impl<T, U> TryInto<U> for T
where U: TryFrom<T>,

source§

type Error = <U as TryFrom<T>>::Error

The type returned in the event of a conversion error.
source§

fn try_into(self) -> Result<U, <U as TryFrom<T>>::Error>

Performs the conversion.
source§

impl<T> WithSubscriber for T

source§

fn with_subscriber<S>(self, subscriber: S) -> WithDispatch<Self>
where S: Into<Dispatch>,

Attaches the provided Subscriber to this type, returning a WithDispatch wrapper. Read more
source§

fn with_current_subscriber(self) -> WithDispatch<Self>

Attaches the current default Subscriber to this type, returning a WithDispatch wrapper. Read more