diff --git a/gazebo/physics/Model.cc b/gazebo/physics/Model.cc index 7d2e816de9..b471b835e3 100644 --- a/gazebo/physics/Model.cc +++ b/gazebo/physics/Model.cc @@ -269,6 +269,29 @@ void Model::LoadJoints() } } +void SetInitialPositionForAxis(JointPtr const& joint, unsigned int axisIdx, std::string const& axisElementName) +{ + sdf::ElementPtr const jointSdf = joint->GetSDF(); + if (joint->DOF() <= axisIdx || !jointSdf->HasElement(axisElementName)) + return; + + sdf::ElementPtr const axisElem = jointSdf->GetElement(axisElementName); + if (axisElem->HasElement("initial_position")) + { + double const initial_position = axisElem->Get("initial_position"); + if (!joint->SetPosition(axisIdx, initial_position)) + { + gzerr << "Failed to set initial position " << initial_position << " for joint " << joint->GetScopedName() << std::endl; + } + } +} + +void SetInitialPosition(JointPtr const& joint) +{ + SetInitialPositionForAxis(joint, 0, "axis"); + SetInitialPositionForAxis(joint, 1, "axis2"); +} + ////////////////////////////////////////////////// void Model::Init() { @@ -314,6 +337,13 @@ void Model::Init() this->jointPub->Publish(msg); } + // This needs to be done after the whole kinematic chain has been initialized. + // Doing this in the joint's Init method completely messes up the setup. + for (auto& joint : this->joints) + { + SetInitialPosition(joint); + } + for (std::vector::iterator iter = this->grippers.begin(); iter != this->grippers.end(); ++iter) { @@ -1350,14 +1380,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); + } + } } ///////////////////////////////////////////////// diff --git a/gazebo/physics/ModelState.cc b/gazebo/physics/ModelState.cc index 574f78d70b..bdaff09884 100644 --- a/gazebo/physics/ModelState.cc +++ b/gazebo/physics/ModelState.cc @@ -204,7 +204,7 @@ void ModelState::Load(const sdf::ElementPtr _elem) } // Set all the joints - /*this->jointStates.clear(); + this->jointStates.clear(); if (_elem->HasElement("joint")) { sdf::ElementPtr childElem = _elem->GetElement("joint"); @@ -215,7 +215,7 @@ void ModelState::Load(const sdf::ElementPtr _elem) JointState(childElem))); childElem = childElem->GetNextElement("joint"); } - }*/ + } } ///////////////////////////////////////////////// diff --git a/gazebo/physics/ModelState_TEST.cc b/gazebo/physics/ModelState_TEST.cc index 85ab70202e..1c64ef38f0 100644 --- a/gazebo/physics/ModelState_TEST.cc +++ b/gazebo/physics/ModelState_TEST.cc @@ -165,6 +165,41 @@ TEST_F(ModelStateTest, Nested) } } +TEST_F(ModelStateTest, JointState) +{ + std::ostringstream sdfStr; + sdfStr << "" + << "" + << "" + << "" + << " " + << " 1.57" + << " " + << "" + << "" + << "" + << ""; + sdf::SDFPtr worldSDF(new sdf::SDF); + worldSDF->SetFromString(sdfStr.str()); + EXPECT_TRUE(worldSDF->Root()->HasElement("world")); + sdf::ElementPtr worldElem = worldSDF->Root()->GetElement("world"); + EXPECT_TRUE(worldElem->HasElement("state")); + sdf::ElementPtr stateElem = worldElem->GetElement("state"); + EXPECT_TRUE(stateElem->HasElement("model")); + sdf::ElementPtr modelElem = stateElem->GetElement("model"); + EXPECT_TRUE(modelElem != nullptr); + + physics::ModelState const modelState(modelElem); + + ASSERT_EQ(modelState.GetJointStateCount(), 1u); + + physics::JointState const jointState = modelState.GetJointState(0); + EXPECT_EQ(1u, jointState.GetAngleCount()); + EXPECT_EQ(1u, jointState.Positions().size()); + EXPECT_DOUBLE_EQ(1.57, jointState.Position(0)); + EXPECT_TRUE(ignition::math::isnan(jointState.Position(1))); +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/integration/model.cc b/test/integration/model.cc index 0d51370f62..5781579f4b 100644 --- a/test/integration/model.cc +++ b/test/integration/model.cc @@ -51,6 +51,34 @@ TEST_F(ModelTest, GetLinksV) EXPECT_EQ(model->GetLinks().size(), 1u); } +TEST_F(ModelTest, LoadJointState) +{ + Load("worlds/joint_state_test.world"); + physics::ModelPtr const model = GetModel("model_3"); + + ASSERT_EQ(model->GetJointCount(), 2u); + + physics::JointPtr const joint_1 = model->GetJoint("joint_1"); + EXPECT_NEAR(joint_1->Position(), 0.7854, 0.0001); + + physics::JointPtr const joint_2 = model->GetJoint("joint_2"); + EXPECT_NEAR(joint_2->Position(), 0.7854, 0.0001); +} + +TEST_F(ModelTest, JointInitialPosition) +{ + Load("worlds/joint_test.world"); + physics::ModelPtr const model = GetModel("model_3"); + + ASSERT_EQ(model->GetJointCount(), 2u); + + physics::JointPtr const joint_1 = model->GetJoint("joint_1"); + EXPECT_NEAR(joint_1->Position(), 0.7854, 0.0001); + + physics::JointPtr const joint_2 = model->GetJoint("joint_2"); + EXPECT_NEAR(joint_2->Position(), 1.5707, 0.0001); +} + ///////////////////////////////////////////////// // This tests getting the scoped name of a model. TEST_F(ModelTest, GetScopedName) diff --git a/test/worlds/joint_state_test.world b/test/worlds/joint_state_test.world new file mode 100644 index 0000000000..03c6381b49 --- /dev/null +++ b/test/worlds/joint_state_test.world @@ -0,0 +1,57 @@ + + + + + + model://sun + + + 0 0 0 0 0 0 + + + 0 0.5 0 0 0 0 + + + 0.125 0.95 0.125 + + + + + + + 0 1.5 0 0 0 0 + + + 0.125 0.95 0.125 + + + + + + world + link_1 + + 0 0 1 + + + + link_1 + link_2 + + 0 0 1 + + 0 1 0 0 0 0 + + + + + + 0.7854 + + + 0.7854 + + + + + diff --git a/test/worlds/joint_test.world b/test/worlds/joint_test.world index 36610eab76..76c2c4e05e 100644 --- a/test/worlds/joint_test.world +++ b/test/worlds/joint_test.world @@ -141,5 +141,45 @@ 0 + + 0 2 0 0 0 0 + + + 0 0.5 0 0 0 0 + + + 0.125 0.95 0.125 + + + + + + + 0 1.5 0 0 0 0 + + + 0.125 0.95 0.125 + + + + + + world + link_1 + + 0 0 1 + 0.7854 + + + + link_1 + link_2 + + 0 0 1 + 1.5707 + + 0 1 0 0 0 0 + +