reset accelerations on setToDefaultValues (#2618)

moveit_core/robot_state/src/robot_state.cpp

@@ -341,6 +341,7 @@ void RobotState::setToDefaultValues()
   robot_model_->getVariableDefaultPositions(position_);  // mimic values are updated
   // set velocity & acceleration to 0
   std::fill(velocity_.begin(), velocity_.end(), 0);
+  std::fill(effort_or_acceleration_.begin(), effort_or_acceleration_.end(), 0);
   std::fill(dirty_joint_transforms_.begin(), dirty_joint_transforms_.end(), 1);
   dirty_link_transforms_ = robot_model_->getRootJoint();
 }

moveit_core/robot_state/test/robot_state_test.cpp

@@ -409,6 +409,34 @@ class OneRobot : public testing::Test
   moveit::core::RobotModelConstPtr robot_model_;
 };
 
+TEST_F(OneRobot, setToDefaultValues)
+{
+  moveit::core::RobotState state(robot_model_);
+  state.setToDefaultValues();
+
+  // Get default values for joint_a.
+  double joint_a_default_position = state.getVariablePosition("joint_a");
+  double joint_a_default_velocity = state.getVariableVelocity("joint_a");
+  double joint_a_default_acceleration = state.getVariableAcceleration("joint_a");
+
+  // Set joint_a to some values.
+  state.setVariablePosition("joint_a", 1.0);
+  state.setVariableVelocity("joint_a", 2.0);
+  state.setVariableAcceleration("joint_a", 3.0);
+
+  // Check that joint_a has the right values.
+  EXPECT_EQ(state.getVariablePosition("joint_a"), 1.0);
+  EXPECT_EQ(state.getVariableVelocity("joint_a"), 2.0);
+  EXPECT_EQ(state.getVariableAcceleration("joint_a"), 3.0);
+
+  // Set to default values.
+  // Check that joint_a is back to the default values.
+  state.setToDefaultValues();
+  EXPECT_EQ(state.getVariablePosition("joint_a"), joint_a_default_position);
+  EXPECT_EQ(state.getVariableVelocity("joint_a"), joint_a_default_velocity);
+  EXPECT_EQ(state.getVariableAcceleration("joint_a"), joint_a_default_acceleration);
+}
+
 TEST_F(OneRobot, FK)
 {
   moveit::core::RobotModelConstPtr model = robot_model_;