Skip to content

Commit

Permalink
Readability: kinematic_state -> robot_state (#2078)
Browse files Browse the repository at this point in the history
  • Loading branch information
@sjahr
sjahr committed Apr 4, 2023
1 parent dd2c409 commit 37c426e
Show file tree
Hide file tree
Showing 5 changed files with 31 additions and 31 deletions.
20 changes: 10 additions & 10 deletions moveit_kinematics/test/benchmark_ik.cpp
View file
Expand Up @@ -84,7 +84,7 @@ int main(int argc, char* argv[])
// const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
const moveit::core::RobotModelPtr& kinematic_model = moveit::core::loadTestingRobotModel("panda");
planning_scene::PlanningScene planning_scene(kinematic_model);
moveit::core::RobotState& kinematic_state = planning_scene.getCurrentStateNonConst();
moveit::core::RobotState& robot_state = planning_scene.getCurrentStateNonConst();
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
std::chrono::duration<double> ik_time(0);
Expand Down Expand Up @@ -121,17 +121,17 @@ int main(int argc, char* argv[])

// perform first IK call to load the cache, so that the time for loading is not included in
// average IK call time
kinematic_state.setToDefaultValues();
robot_state.setToDefaultValues();
EigenSTL::vector_Isometry3d default_eef_states;
for (const auto& end_effector : end_effectors)
default_eef_states.push_back(kinematic_state.getGlobalLinkTransform(end_effector));
default_eef_states.push_back(robot_state.getGlobalLinkTransform(end_effector));
if (end_effectors.size() == 1)
{
kinematic_state.setFromIK(group, default_eef_states[0], end_effectors[0], 0.1);
robot_state.setFromIK(group, default_eef_states[0], end_effectors[0], 0.1);
}
else
{
kinematic_state.setFromIK(group, default_eef_states, end_effectors, 0.1);
robot_state.setFromIK(group, default_eef_states, end_effectors, 0.1);
}

bool found_ik;
Expand All @@ -140,7 +140,7 @@ int main(int argc, char* argv[])
unsigned int i = 0;
while (i < num)
{
kinematic_state.setToRandomPositions(group);
robot_state.setToRandomPositions(group);
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
if (collision_result.collision)
Expand All @@ -149,17 +149,17 @@ int main(int argc, char* argv[])
continue;
}
for (unsigned j = 0; j < end_effectors.size(); ++j)
end_effector_states[j] = kinematic_state.getGlobalLinkTransform(end_effectors[j]);
end_effector_states[j] = robot_state.getGlobalLinkTransform(end_effectors[j]);
if (reset_to_default)
kinematic_state.setToDefaultValues();
robot_state.setToDefaultValues();
start = std::chrono::system_clock::now();
if (end_effectors.size() == 1)
{
found_ik = kinematic_state.setFromIK(group, end_effector_states[0], end_effectors[0], 0.1);
found_ik = robot_state.setFromIK(group, end_effector_states[0], end_effectors[0], 0.1);
}
else
{
found_ik = kinematic_state.setFromIK(group, end_effector_states, end_effectors, 0.1);
found_ik = robot_state.setFromIK(group, end_effector_states, end_effectors, 0.1);
}
ik_time += std::chrono::system_clock::now() - start;
if (!found_ik)
Expand Down
4 changes: 2 additions & 2 deletions .../rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/planning_link_updater.h
View file
Expand Up @@ -45,7 +45,7 @@ namespace moveit_rviz_plugin
class PlanningLinkUpdater : public rviz_default_plugins::robot::LinkUpdater
{
public:
PlanningLinkUpdater(const moveit::core::RobotStateConstPtr& state) : kinematic_state_(state)
PlanningLinkUpdater(const moveit::core::RobotStateConstPtr& state) : robot_state_(state)
{
}

Expand All @@ -54,6 +54,6 @@ class PlanningLinkUpdater : public rviz_default_plugins::robot::LinkUpdater
Ogre::Quaternion& collision_orientation) const override;

private:
moveit::core::RobotStateConstPtr kinematic_state_;
moveit::core::RobotStateConstPtr robot_state_;
};
} // namespace moveit_rviz_plugin
10 changes: 5 additions & 5 deletions ...z_plugin_render_tools/include/moveit/rviz_plugin_render_tools/robot_state_visualization.h
View file
Expand Up @@ -61,13 +61,13 @@ class RobotStateVisualization
void load(const urdf::ModelInterface& descr, bool visual = true, bool collision = true);
void clear();

void update(const moveit::core::RobotStateConstPtr& kinematic_state);
void update(const moveit::core::RobotStateConstPtr& kinematic_state,
void update(const moveit::core::RobotStateConstPtr& robot_state);
void update(const moveit::core::RobotStateConstPtr& robot_state,
const std_msgs::msg::ColorRGBA& default_attached_object_color);
void update(const moveit::core::RobotStateConstPtr& kinematic_state,
void update(const moveit::core::RobotStateConstPtr& robot_state,
const std_msgs::msg::ColorRGBA& default_attached_object_color,
const std::map<std::string, std_msgs::msg::ColorRGBA>& color_map);
void updateKinematicState(const moveit::core::RobotStateConstPtr& kinematic_state);
void updateKinematicState(const moveit::core::RobotStateConstPtr& robot_state);
void setDefaultAttachedObjectColor(const std_msgs::msg::ColorRGBA& default_attached_object_color);
/// update color of all attached object shapes
void updateAttachedObjectColors(const std_msgs::msg::ColorRGBA& attached_object_color);
Expand Down Expand Up @@ -98,7 +98,7 @@ class RobotStateVisualization
void setAlpha(float alpha);

private:
void updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state,
void updateHelper(const moveit::core::RobotStateConstPtr& robot_state,
const std_msgs::msg::ColorRGBA& default_attached_object_color,
const std::map<std::string, std_msgs::msg::ColorRGBA>* color_map);
rviz_default_plugins::robot::Robot robot_;
Expand Down
6 changes: 3 additions & 3 deletions moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp
View file
Expand Up @@ -43,16 +43,16 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin
Ogre::Vector3& collision_position,
Ogre::Quaternion& collision_orientation) const
{
const moveit::core::LinkModel* link_model = kinematic_state_->getLinkModel(link_name);
const moveit::core::LinkModel* link_model = robot_state_->getLinkModel(link_name);

if (!link_model)
{
return false;
}

// getGlobalLinkTransform() returns a valid isometry by contract
const Eigen::Vector3d& robot_visual_position = kinematic_state_->getGlobalLinkTransform(link_model).translation();
Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).linear());
const Eigen::Vector3d& robot_visual_position = robot_state_->getGlobalLinkTransform(link_model).translation();
Eigen::Quaterniond robot_visual_orientation(robot_state_->getGlobalLinkTransform(link_model).linear());
visual_position = Ogre::Vector3(robot_visual_position.x(), robot_visual_position.y(), robot_visual_position.z());
visual_orientation = Ogre::Quaternion(robot_visual_orientation.w(), robot_visual_orientation.x(),
robot_visual_orientation.y(), robot_visual_orientation.z());
Expand Down
22 changes: 11 additions & 11 deletions moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp
View file
Expand Up @@ -89,33 +89,33 @@ void RobotStateVisualization::updateAttachedObjectColors(const std_msgs::msg::Co
robot_.getAlpha());
}

void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state)
void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& robot_state)
{
updateHelper(kinematic_state, default_attached_object_color_, nullptr);
updateHelper(robot_state, default_attached_object_color_, nullptr);
}

void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state,
void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& robot_state,
const std_msgs::msg::ColorRGBA& default_attached_object_color)
{
updateHelper(kinematic_state, default_attached_object_color, nullptr);
updateHelper(robot_state, default_attached_object_color, nullptr);
}

void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& kinematic_state,
void RobotStateVisualization::update(const moveit::core::RobotStateConstPtr& robot_state,
const std_msgs::msg::ColorRGBA& default_attached_object_color,
const std::map<std::string, std_msgs::msg::ColorRGBA>& color_map)
{
updateHelper(kinematic_state, default_attached_object_color, &color_map);
updateHelper(robot_state, default_attached_object_color, &color_map);
}

void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPtr& kinematic_state,
void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPtr& robot_state,
const std_msgs::msg::ColorRGBA& default_attached_object_color,
const std::map<std::string, std_msgs::msg::ColorRGBA>* color_map)
{
robot_.update(PlanningLinkUpdater(kinematic_state));
robot_.update(PlanningLinkUpdater(robot_state));
render_shapes_->clear();

std::vector<const moveit::core::AttachedBody*> attached_bodies;
kinematic_state->getAttachedBodies(attached_bodies);
robot_state->getAttachedBodies(attached_bodies);
for (const moveit::core::AttachedBody* attached_body : attached_bodies)
{
std_msgs::msg::ColorRGBA color = default_attached_object_color;
Expand Down Expand Up @@ -153,9 +153,9 @@ void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPt
robot_.setVisible(visible_);
}

void RobotStateVisualization::updateKinematicState(const moveit::core::RobotStateConstPtr& kinematic_state)
void RobotStateVisualization::updateKinematicState(const moveit::core::RobotStateConstPtr& robot_state)
{
robot_.update(PlanningLinkUpdater(kinematic_state));
robot_.update(PlanningLinkUpdater(robot_state));
}

void RobotStateVisualization::setVisible(bool visible)
Expand Down

0 comments on commit 37c426e

Please sign in to comment.