Ignition Gazebo: set joint position

edit

Hi

I want to manually set a joint position in my plugin for Ignition and trying to figure out how to do it. I've tried to set it through the JointPosition component but with no success.

What I did:
1. Found joint by the name
2. Created JointPosition component for this joint
3. Used SetData() to set new joint position
4. Used SetChanged to force an update on the client side

Entity joint = _ecm.EntityByComponents(
  components::ParentEntity(this->ownerEntity),
  components::Name(jointName),
  components::Joint());

auto jointPosComp = _ecm.Component<components::JointPosition>(joint);
if (jointPosComp == nullptr)
{  
  _ecm.CreateComponent(joint,components::JointPosition());
  ignmsg << "jointPosComp created" << std::endl;
  _ecm.CreateComponent(joint, components::JointVelocity());
  return;
}

ignerr << "jointPosComp: " << jointPosComp->Data()[0] << std::endl;

jointPosComp->SetData(std::vector<double>{newJointPos},
      [](const std::vector<double> &, const std::vector<double> &) -> bool
      {
        // No check for equality - always return false
        return false;
      });
_ecm.SetChanged(this->ownerEntity,components::JointPosition::typeId,ComponentState::OneTimeChange);

When I run it in PreUpdate section, even though I change JointPosition, I still get a JointPosition of 0 on the next iteration.

Any suggestions how to manually change joint position?

Thx