gear

Rust-lang中的机器人避障路径规划

文档

代码示例

最小代码示例

extern crate gear;
extern crate nalgebra as na;

fn main() {
    // Create path planner with loading urdf file and set end link name
    let planner = gear::JointPathPlannerBuilder::from_urdf_file("sample.urdf")
        .expect("failed to create planner from urdf file")
        .collision_check_margin(0.01)
        .finalize();
    // Create inverse kinematics solver
    let solver = gear::JacobianIKSolverBuilder::<f64>::new()
        .num_max_try(1000)
        .allowable_target_distance(0.01)
        .move_epsilon(0.0001)
        .finalize();
    let solver = gear::RandomInitializeIKSolver::new(solver, 100);
    // Create path planner with IK solver
    let mut planner = gear::JointPathPlannerWithIK::new(planner, solver);
    // Create kinematic chain from the end of the link
    let mut arm = planner.create_arm("l_wrist2").unwrap();

    // Create obstacles
    let obstacles =
        gear::create_compound_from_urdf("obstacles.urdf").expect("obstacle file not found");

    // Set IK target transformation
    let mut ik_target_pose = na::Isometry3::from_parts(
        na::Translation3::new(0.40, 0.20, 0.3),
        na::UnitQuaternion::from_euler_angles(0.0, -0.1, 0.0),
    );
    // Plan the path, path is the vector of joint angles for root to "l_wrist2"
    let plan1 = planner
        .plan_with_ik(&mut arm, &ik_target_pose, &obstacles)
        .unwrap();
    println!("plan1 = {:?}", plan1);
    ik_target_pose.translation.vector[2] += 0.50;
    // plan the path from previous result
    let plan2 = planner
        .plan_with_ik(&mut arm, &ik_target_pose, &obstacles)
        .unwrap();
    println!("plan2 = {:?}", plan2);
}

使用GUI运行示例

如何运行

cargo run --release --example reach

GUI控制

• 上/下/左/右/f/b以转换逆运动学目标
• Shift + 上/下/左/右/f/b以旋转逆运动学目标
• 输入g将机械臂末端移动到目标位置
• 输入i仅为目标求解逆运动学，不进行碰撞检查
• 输入r设置随机姿态
• 输入c检查碰撞
• 输入v切换显示元素碰撞＜-＞视觉

使用您的机器人

示例可以处理任何urdf文件（默认使用sample.urdf）。它需要目标末端链接的名称。

cargo run --release --example reach YOUR_URDF_FILE_PATH END_LINK_NAME

例如，

PR2

cargo run --release --example reach $(rospack find pr2_description)/robots/pr2.urdf.xacro l_gripper_palm_link

通用机器人：UR10

cargo run --release --example reach $(rospack find ur_description)/urdf/ur10_robot.urdf.xacro ee_link

Sawyer

cargo run --release --example reach $(rospack find sawyer_description)/urdf/sawyer.urdf right_hand