From 0b903d6d1e0fcba04601c99bc7d76f8cd0f63634 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20B=C3=BCttner?= Date: Fri, 8 Jan 2021 13:56:34 +0100 Subject: [PATCH] Apply the loaded joint state Use the commented code and update it to the new API. Also extend it for more than one joint axis. --- gazebo/physics/Model.cc | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gazebo/physics/Model.cc b/gazebo/physics/Model.cc index 7d2e816de9..f89782a30f 100644 --- a/gazebo/physics/Model.cc +++ b/gazebo/physics/Model.cc @@ -1350,14 +1350,14 @@ void Model::SetState(const ModelState &_state) gzerr << "Unable to find model[" << ms.first << "]\n"; } - // For now we don't use the joint state values to set the state of - // simulation. - // for (unsigned int i = 0; i < _state.GetJointStateCount(); ++i) - // { - // JointState jointState = _state.GetJointState(i); - // this->SetJointPosition(this->GetName() + "::" + jointState.GetName(), - // jointState.GetAngle(0).Radian()); - // } + for (unsigned int i = 0; i < _state.GetJointStateCount(); ++i) + { + JointState const jointState = _state.GetJointState(i); + for (unsigned int k = 0; k < jointState.GetAngleCount(); ++k) { + this->SetJointPosition(this->GetName() + "::" + jointState.GetName(), + jointState.Position(k), k); + } + } } /////////////////////////////////////////////////