Merge branch 'gz-sim8' into feature/break-out-server-constructor

src/systems/physics/Physics.cc

@@ -2404,6 +2404,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
  << std::endl;
  return true;
  }
+ math::Pose3d worldPoseCmd = _poseCmd->Data();
+ if (!worldPoseCmd.Pos().IsFinite() || !worldPoseCmd.Rot().IsFinite() ||
+ worldPoseCmd.Rot() == math::Quaterniond::Zero)
+ {
+ gzerr << "Unable to set world pose. Invalid pose value: "
+ << worldPoseCmd << std::endl;
+ return true;
+ }
 
  // TODO(addisu) Store the free group instead of searching for it at
  // every iteration
@@ -2423,7 +2431,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
  math::Pose3d linkPose =
  this->RelativePose(_entity, linkEntity, _ecm);
 
- freeGroup->SetWorldPose(math::eigen3::convert(_poseCmd->Data() *
+ freeGroup->SetWorldPose(math::eigen3::convert(worldPoseCmd *
  linkPose));
 
  // Process pose commands for static models here, as one-time changes
@@ -2432,7 +2440,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
  auto worldPoseComp = _ecm.Component<components::Pose>(_entity);
  if (worldPoseComp)
  {
- auto state = worldPoseComp->SetData(_poseCmd->Data(),
+ auto state = worldPoseComp->SetData(worldPoseCmd,
  this->pose3Eql) ?
  ComponentState::OneTimeChange :
  ComponentState::NoChange;