Set robot to position without planning

edit

Found out the answer after some more web research:

Basically get the current state, set the joints to whatever position you like and call setStartState:

moveit::planning_interface::MoveGroupInterface m_moveGroup("arm_group");
moveit::planning_interface::PlanningSceneInterface m_planningSceneInterface;
moveit::planning_interface::MoveGroupInterface::Plan m_plan;
PlanInterface planInterface(&m_moveGroup, &m_planningSceneInterface, &m_plan);
planInterface.setJointConstraints();

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
planning_scene::PlanningScene planning_scene(kinematic_model);

robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();
std::vector<double> joint_values = { 1.57, 1.57, 1.57, 1.57, 1.57, 1.57 };
const robot_model::JointModelGroup* joint_model_group = current_state.getJointModelGroup("arm_group");
current_state.setJointGroupPositions(joint_model_group, joint_values);
m_moveGroup.setStartState(current_state);

Now the planner will use joint_values as the start pose to plan from. In Rviz however teh start state will not change. Only once the planner start the trajectory will become visible with the starts state.