Merge pull request #528 from christian-rauch/controlled_names

distinguish between controlled and model joints and links
ipab-slmc · Mar 12, 2019 · c815fcd · c815fcd
2 parents b3f9bbb + 19ca700
commit c815fcd
Show file tree

Hide file tree

Showing 7 changed files with 30 additions and 11 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -0,0 +1,11 @@
+# Changelog
+
+## [Unreleased]
+### Changed
+- renamed KinematicTree::GetJointNames -> KinematicTree::GetControlledJointNames
+- renamed Scene::GetJointNames -> Scene::GetControlledJointNames
+- pyexotica: get_joint_names -> get_controlled_joint_names
+
+### Added
+- pyexotica: get_model_link_names, get_controlled_link_names
+- pyexotica: get_model_joint_names, get_controlled_joint_names
diff --git a/exotica_core/include/exotica_core/kinematic_tree.h b/exotica_core/include/exotica_core/kinematic_tree.h
@@ -153,7 +153,7 @@ class KinematicTree : public Uncopyable
     int GetNumControlledJoints();
     int GetNumModelJoints();
     void PublishFrames();
-    std::vector<std::string> GetJointNames()
+    std::vector<std::string> GetControlledJointNames()
     {
         return controlled_joints_names_;
     }

diff --git a/exotica_core/include/exotica_core/scene.h b/exotica_core/include/exotica_core/scene.h
@@ -76,8 +76,8 @@ class Scene : public Object, Uncopyable, public Instantiable<SceneInitializer>
     std::string GetRootJointName();
     moveit_msgs::PlanningScene GetPlanningSceneMsg();
     exotica::KinematicTree& GetKinematicTree();
-    void GetJointNames(std::vector<std::string>& joints);
-    std::vector<std::string> GetJointNames();
+    void GetControlledJointNames(std::vector<std::string>& joints);
+    std::vector<std::string> GetControlledJointNames();
     std::vector<std::string> GetModelJointNames();
     std::vector<std::string> GetControlledLinkNames();
     std::vector<std::string> GetModelLinkNames();

diff --git a/exotica_core/src/planning_problem.cpp b/exotica_core/src/planning_problem.cpp
@@ -72,7 +72,7 @@ void PlanningProblem::SetStartState(Eigen::VectorXdRefConst x)
     }
     else if (x.rows() == scene_->GetKinematicTree().GetNumControlledJoints())
     {
-        std::vector<std::string> jointNames = scene_->GetJointNames();
+        std::vector<std::string> jointNames = scene_->GetControlledJointNames();
         std::vector<std::string> modelNames = scene_->GetModelJointNames();
         for (int i = 0; i < jointNames.size(); ++i)
         {

diff --git a/exotica_core/src/scene.cpp b/exotica_core/src/scene.cpp
@@ -405,14 +405,14 @@ exotica::KinematicTree& Scene::GetKinematicTree()
     return kinematica_;
 }
 
-void Scene::GetJointNames(std::vector<std::string>& joints)
+void Scene::GetControlledJointNames(std::vector<std::string>& joints)
 {
-    joints = kinematica_.GetJointNames();
+    joints = kinematica_.GetControlledJointNames();
 }
 
-std::vector<std::string> Scene::GetJointNames()
+std::vector<std::string> Scene::GetControlledJointNames()
 {
-    return kinematica_.GetJointNames();
+    return kinematica_.GetControlledJointNames();
 }
 
 std::vector<std::string> Scene::GetControlledLinkNames()

diff --git a/exotica_core/src/visualization.cpp b/exotica_core/src/visualization.cpp
@@ -144,7 +144,7 @@ void Visualization::DisplayTrajectory(Eigen::MatrixXdRefConst trajectory)
     traj_msg.trajectory[0].joint_trajectory.points.resize(num_trajectory_points);
     traj_msg.trajectory[0].joint_trajectory.joint_names.resize(num_actuated_joints_without_base);
     for (int i = 0; i < num_actuated_joints_without_base; ++i)
-        traj_msg.trajectory[0].joint_trajectory.joint_names[i] = scene_->GetKinematicTree().GetJointNames()[base_offset + i];
+        traj_msg.trajectory[0].joint_trajectory.joint_names[i] = scene_->GetKinematicTree().GetControlledJointNames()[base_offset + i];
 
     // Insert actuated joints without base
     for (int i = 0; i < num_trajectory_points; ++i)
@@ -158,4 +158,4 @@ void Visualization::DisplayTrajectory(Eigen::MatrixXdRefConst trajectory)
 
     trajectory_pub_.publish(traj_msg);
 }
-}
+}
diff --git a/exotica_python/src/pyexotica.cpp b/exotica_python/src/pyexotica.cpp
@@ -973,7 +973,7 @@ PYBIND11_MODULE(_pyexotica, module)
     scene.def("update", &Scene::Update, py::arg("x"), py::arg("t") = 0.0);
     scene.def("get_base_type", &Scene::GetBaseType);
     scene.def("get_group_name", &Scene::GetGroupName);
-    scene.def("get_joint_names", (std::vector<std::string>(Scene::*)()) & Scene::GetJointNames);
+    scene.def("get_controlled_joint_names", (std::vector<std::string>(Scene::*)()) & Scene::GetControlledJointNames);
     scene.def("get_controlled_link_names", &Scene::GetControlledLinkNames);
     scene.def("get_model_link_names", &Scene::GetModelLinkNames);
     scene.def("get_kinematic_tree", &Scene::GetKinematicTree, py::return_value_policy::reference_internal);
@@ -1106,6 +1106,14 @@ PYBIND11_MODULE(_pyexotica, module)
     kinematic_tree.def("get_num_model_joints", &KinematicTree::GetNumModelJoints);
     kinematic_tree.def("get_num_controlled_joints", &KinematicTree::GetNumControlledJoints);
 
+    // joints and links that describe the full state of the robot
+    kinematic_tree.def("get_model_link_names", &KinematicTree::GetModelLinkNames);
+    kinematic_tree.def("get_model_joint_names", &KinematicTree::GetModelJointNames);
+
+    // subset of model joints and links that can be controlled
+    kinematic_tree.def("get_controlled_link_names", &KinematicTree::GetControlledLinkNames);
+    kinematic_tree.def("get_controlled_joint_names", &KinematicTree::GetControlledJointNames);
+
     // Joint Limits
     kinematic_tree.def("get_joint_limits", &KinematicTree::GetJointLimits);
     kinematic_tree.def("reset_joint_limits", &KinematicTree::ResetJointLimits);