Struct k::JacobianIkSolver [−][src]
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>,
[src]
T: RealField + SubsetOf<f64>,
pub fn new(
allowable_target_distance: T,
allowable_target_angle: T,
jacobian_multiplier: T,
num_max_try: usize
) -> JacobianIkSolver<T>
[src]
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> + Send + Sync>
)
[src]
&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], ), ));
pub fn clear_nullspace_function(&mut self)
[src]
Clear the null function which is set by set_nullspace_funtion
.
Trait Implementations
impl<T> Default for JacobianIkSolver<T> where
T: RealField + SubsetOf<f64>,
[src]
T: RealField + SubsetOf<f64>,
impl<T> InverseKinematicsSolver<T> for JacobianIkSolver<T> where
T: RealField + SubsetOf<f64>,
[src]
T: RealField + SubsetOf<f64>,
fn solve(
&self,
arm: &SerialChain<T>,
target_pose: &Isometry3<T>
) -> Result<(), Error>
[src]
&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());
fn solve_with_constraints(
&self,
arm: &SerialChain<T>,
target_pose: &Isometry3<T>,
constraints: &Constraints
) -> Result<(), Error>
[src]
&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> !RefUnwindSafe for JacobianIkSolver<T>
[src]
impl<T> Send for JacobianIkSolver<T>
[src]
impl<T> Sync for JacobianIkSolver<T>
[src]
impl<T> Unpin for JacobianIkSolver<T> where
T: Unpin,
[src]
T: Unpin,
impl<T> !UnwindSafe for JacobianIkSolver<T>
[src]
Blanket Implementations
impl<T> Any for T where
T: 'static + ?Sized,
[src]
T: 'static + ?Sized,
impl<T> Borrow<T> for T where
T: ?Sized,
[src]
T: ?Sized,
impl<T> BorrowMut<T> for T where
T: ?Sized,
[src]
T: ?Sized,
pub fn borrow_mut(&mut self) -> &mut T
[src]
impl<T> From<T> for T
[src]
impl<T, U> Into<U> for T where
U: From<T>,
[src]
U: From<T>,
impl<T> Same<T> for T
type Output = T
Should always be Self
impl<SS, SP> SupersetOf<SS> for SP where
SS: SubsetOf<SP>,
[src]
SS: SubsetOf<SP>,
pub fn to_subset(&self) -> Option<SS>
[src]
pub fn is_in_subset(&self) -> bool
[src]
pub fn to_subset_unchecked(&self) -> SS
[src]
pub fn from_subset(element: &SS) -> SP
[src]
impl<T, U> TryFrom<U> for T where
U: Into<T>,
[src]
U: Into<T>,
type Error = Infallible
The type returned in the event of a conversion error.
pub fn try_from(value: U) -> Result<T, <T as TryFrom<U>>::Error>
[src]
impl<T, U> TryInto<U> for T where
U: TryFrom<T>,
[src]
U: TryFrom<T>,
type Error = <U as TryFrom<T>>::Error
The type returned in the event of a conversion error.
pub fn try_into(self) -> Result<U, <U as TryFrom<T>>::Error>
[src]
impl<V, T> VZip<V> for T where
V: MultiLane<T>,
V: MultiLane<T>,