diff --git a/getParam.md b/getParam.md new file mode 100644 index 0000000..5aca638 --- /dev/null +++ b/getParam.md @@ -0,0 +1,12 @@ +# 加载参数进参数服务器 +``` + +``` +先要在.launch文件里load参数文件 +# 在程序中调用 +通常加载后直接使用相同名字的值就可以 +``` +getParam(nh, " ", 1); +``` +这一条命令就可以直接作为一个具体的值 \ No newline at end of file diff --git "a/manual\344\273\245\345\217\212movelt\345\255\246\344\271\240\347\254\224\350\256\260.md" "b/manual\344\273\245\345\217\212movelt\345\255\246\344\271\240\347\254\224\350\256\260.md" index d069a6a..a7aa024 100644 --- "a/manual\344\273\245\345\217\212movelt\345\255\246\344\271\240\347\254\224\350\256\260.md" +++ "b/manual\344\273\245\345\217\212movelt\345\255\246\344\271\240\347\254\224\350\256\260.md" @@ -84,3 +84,151 @@ const moveit::core::JointModelGroup* joint_model_group = move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP); ``` +#### 获取信息 +利用ROS_INFO_NAMED获取机器人的参考系和执行器link名称还可以获得机器人所有族的名称 + +#### 开始演示 +``` +visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); +``` + +##### Planning the goal +本质上使用以下模板是设定末端执行器姿势的goal坐标 +```c++ +geometry_msgs::Pose target_pose1; +target_pose1.orientation.w = 1.0; +target_pose1.position.x = 0.28; +target_pose1.position.y = -0.2; +target_pose1.position.z = 0.5; +move_group_interface.setPoseTarget(target_pose1); +``` +然后调用planner计算轨迹,同时这一步骤也储存了刚刚写的计划组 +```c++ +moveit::planning_interface::MoveGroupInterface::Plan my_plan; + +bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + +ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); +``` +##### Visualizing plans +下一段代码就是用于在rivz上进行可视化可称为Visualizing plans +```c++ +ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line"); +visual_tools.publishAxisLabeled(target_pose1, "pose1"); +visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); +visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); +visual_tools.trigger(); +visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); +``` +##### Executing +后调用进行计算 +```c++ +move_group_interface.execute(my_plan); +``` +**以上便是计划并且进行计划的代码解释** +最后进行移动 +```c++ +move_group_interface.move(); +``` +**总结:此之上就是调用各个接口在代码层面实现计划并且运动的方法,使用*two-step plan+execute*的方法** + +### 设定joint space +设定一个指针,用于指向储存joint group的加速度/速度/位置的向量 +```c++ +moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState(); +``` +后创建向量使用接口获取所有的数据,使指针指向这个向量 +```c++ +std::vector joint_group_positions; +current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); +``` +修改了一个关节的值时候在进行可视化 +```c++ +joint_group_positions[0] = -tau / 6; // -1/6 turn in radians +move_group_interface.setJointValueTarget(joint_group_positions); +``` +此段用于设置加速度和线速度,或者也可用moveit_config里的joint_limits.yaml里进行修改,或是在rviz中都可修改 +```c++ +move_group_interface.setMaxVelocityScalingFactor(0.05); +move_group_interface.setMaxAccelerationScalingFactor(0.05); + +success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); +ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); +``` +在rviz上可视化的代码与上面提到的基本一致 +```c++ +visual_tools.deleteAllMarkers(); +visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); +visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); +visual_tools.trigger(); +visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); +``` +之后为了让其正常运行,要定义实际上存在的物理量影响因素(可能?)此为一个赋值的过程 +```c++ +moveit_msgs::OrientationConstraint ocm; + ocm.link_name = "panda_link7"; + ocm.header.frame_id = "panda_link0"; + ocm.orientation.w = 1.0; + ocm.absolute_x_axis_tolerance = 0.1;//hixbox? + ocm.absolute_y_axis_tolerance = 0.1; + ocm.absolute_z_axis_tolerance = 0.1; + ocm.weight = 1.0; +``` +后便是要将其设置为实际的tolerance +```c++ +moveit_msgs::Constraints test_constraints; +test_constraints.orientation_constraints.push_back(ocm); +move_group_interface.setPathConstraints(test_constraints);//the key codes +``` +### Enforce Planning in Joint Space +在Moveit中有两种计划问题的方式分别为*joint space*和*cartesian space*在参数文件**ompl_planning.yaml**中修改 +```xml +enforce_joint_model_state_space:true +``` +可以强制所有的计划都使用*joint space*模式 + +#### 两种模式具有的特点 +##### *cartesian space* +作为默认模式具有方向约束采样(不太懂啥意思),便可以调用IK来作为采样生成器 +##### *joint space* +其拒绝采样,所以计划时间会被大大增加 + +#### Enforcing +因为要应用就的目标计划,所以重新设定开始位置 +```c++ +moveit::core::RobotState start_state(*move_group_interface.getCurrentState()); +geometry_msgs::Pose start_pose2; +start_pose2.orientation.w = 1.0; +start_pose2.position.x = 0.55; +start_pose2.position.y = -0.05; +start_pose2.position.z = 0.8; +start_state.setFromIK(joint_model_group, start_pose2); +move_group_interface.setStartState(start_state); +``` +直接应用 +```c++ +move_group_interface.setPoseTarget(target_pose1); +``` +后将计划时间设定为10s让其能够有充足时间计算完成 +```c++ +move_group_interface.setPlanningTime(10.0); + +success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); +ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); +``` +最后在Rviz中可视化 +```c++ +visual_tools.deleteAllMarkers(); +visual_tools.publishAxisLabeled(start_pose2, "start"); +visual_tools.publishAxisLabeled(target_pose1, "goal"); +visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); +visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); +visual_tools.trigger(); +visual_tools.prompt("next step"); +``` +用path constraint完成后记得及时清除 +```c++ +move_group_interface.clearPathConstraints(); +``` + +