Robot Jacobians
In this tutorial, I will overview how to compute the jacobian matrix of a RobotSet.
Example code can be found here:
extern crate optima; use nalgebra::DVector; use optima::robot_modules::robot_kinematics_module::{JacobianEndPoint, JacobianMode}; use optima::robot_set_modules::robot_set::RobotSet; use optima::utils::utils_robot::robot_module_utils::RobotNames; fn main() { // Create a base configuration ur5. let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5")]); // Spawn joint state with zeros for all degrees of freedom. let joint_state = robot_set.spawn_robot_set_joint_state(DVector::from_vec(vec![0.0; 6])).expect("error"); // Computes the full jacobian matrix with end link idx of 9. The `robot_jacobian_end_point` in // this case is simply the link center. let full_jacobian_matrix = robot_set.robot_set_kinematics_module().compute_jacobian(&joint_state, 0, None, 9, &JacobianEndPoint::Link, None, JacobianMode::Full).expect("error"); println!("{}", full_jacobian_matrix); println!("////////////////////////////////////////////////////////////////////////////////////"); // Computes the 3 x N (N = 6 for the ur5 robot) translation jacobian matrix with end link idx of 9. // The `robot_jacobian_end_point` in this case is simply the link center. let translational_jacobian_matrix = robot_set.robot_set_kinematics_module().compute_jacobian(&joint_state, 0, None, 9, &JacobianEndPoint::Link, None, JacobianMode::Translational).expect("error"); println!("{}", translational_jacobian_matrix); println!("////////////////////////////////////////////////////////////////////////////////////"); // Load the test configuration created in the previous tutorial. // Make sure that you ran example 3_robot_set_configurations prior to starting this tutorial. let robot_set = RobotSet::new_from_set_name("test_configuration"); // Spawn joint state with zeros for all degrees of freedom. let joint_state = robot_set.spawn_robot_set_joint_state(DVector::from_vec(vec![0.0; 16])).expect("error"); // Computes the 6 x N (N = 16 for this configuration) full jacobian matrix with end link idx of 18 on robot 1 // (sawyer, in this case of this `RobotSet`). // The `robot_jacobian_end_point` in this case is the inertial origin of the link. let full_jacobian_matrix = robot_set.robot_set_kinematics_module().compute_jacobian(&joint_state, 1, None, 18, &JacobianEndPoint::InertialOrigin, None, JacobianMode::Full).expect("error"); println!("{}", full_jacobian_matrix); println!("////////////////////////////////////////////////////////////////////////////////////"); }
Jacobian matrices are outputted as nalgebra DMatrix objects. These objects are compatible and can multiply with DVector objects (that are often used throughout the rest of the library).
The code for this tutorial can be found in the file optima_toolbox/optima/examples/6_jacobians.rs and can be run from the optima_toolbox/optima directory using the following command:
cargo run --example 6_jacobians
The output of this example should be the following:
┌ ┐
│ -0.19145 0.9118999999999999 0.4868999999999999 0.0946499999999999 -0.08230000000000001 0 │
│ 0 0 0 0 0 0 │
│ 0 0 0 0 0 0 │
│ 0 0 0 0 0 0 │
│ 0 1 1 1 0 1 │
│ 1 0 0 0 1 0 │
└ ┘
////////////////////////////////////////////////////////////////////////////////////
┌ ┐
│ -0.19145 0.9118999999999999 0.4868999999999999 0.0946499999999999 -0.08230000000000001 0 │
│ 0 0 0 0 0 0 │
│ 0 0 0 0 0 0 │
└ ┘
////////////////////////////////////////////////////////////////////////////////////
┌ ┐
│ 0 0 0 0 0 0 0 0 -0.16029998841362336 0 -0.00000018809419351637482 0.00000000000015770399553929073 -0.00000018809615244339527 -0.0000000000006676236440362641 -0.00000018809811155149317 -0.00000000000008510472191613595 │
│ 0 0 0 0 0 0 0 0 1.0155000099998726 0 0.0000000000000006234230649834017 0.0000001880987702344505 0.0000000000000006536766262225693 0.00000018809877014091413 0.0000000000000002398410089369438 0.000000008111625825649074 │
│ 0 0 0 0 0 0 0 0 0 0 -0.934500009999332 -0.03220001158569105 -0.5345000099991265 0.13629998841492075 -0.13450000999927902 -0.000000011584538162702774 │
│ 0 0 0 0 0 0 0 0 0 0 0.0000000000048965276278067904 0.9999999999999992 0.000000000004896361094353097 0.9999999999999986 0.000000000004896361094353097 0.9999999999730148 │
│ 0 0 0 0 0 0 0 0 0 0 0.9999999999999996 -0.0000000000000000000000000003844217798755143 0.9999999999999989 0.00000000000000007850462293399649 0.9999999999999984 -0.00000000000007473188734508085 │
│ 0 0 0 0 0 0 0 0 1 0 0.0000000000000006661338147750939 0.0000000000048976378508314156 0.0000000000000012212453270876722 0.000000000004898192962343728 0.0000000000000017763568394002505 -0.000007346406160069208 │
└ ┘
////////////////////////////////////////////////////////////////////////////////////