Skip to content

Commit

Permalink
More moveit_py tidy fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
sjahr committed Oct 27, 2023
1 parent 090fc6e commit f85727b
Show file tree
Hide file tree
Showing 5 changed files with 21 additions and 21 deletions.
2 changes: 1 addition & 1 deletion moveit_py/src/moveit/core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ PYBIND11_MODULE(core, m)
moveit_py::bind_planning_scene::initPlanningScene(m);
moveit_py::bind_planning_interface::initMotionPlanResponse(m);
moveit_py::bind_robot_model::initJointModel(m);
moveit_py::bind_robot_model::initJointModel_group(m);
moveit_py::bind_robot_model::initJointModelGroup(m);
moveit_py::bind_robot_model::initRobotModel(m);
moveit_py::bind_robot_state::initRobotState(m);
moveit_py::bind_robot_trajectory::initRobotTrajectory(m);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ bool satisfiesPositionBounds(const moveit::core::JointModelGroup* jmg, const Eig
return jmg->satisfiesPositionBounds(joint_positions.data(), margin);
}

void initJointModel_group(py::module& m)
void initJointModelGroup(py::module& m)
{
py::module robot_model = m.def_submodule("robot_model");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,6 @@ namespace bind_robot_model
{
bool satisfiesPositionBounds(const moveit::core::JointModelGroup* jmg, const Eigen::VectorXd& joint_positions,
const double margin);
void initJointModel_group(py::module& m);
void initJointModelGroup(py::module& m);
} // namespace bind_robot_model
} // namespace moveit_py
32 changes: 16 additions & 16 deletions moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ geometry_msgs::msg::Pose getPose(const moveit::core::RobotState* self, const std
return tf2::toMsg(self->getGlobalLinkTransform(link_name));
}

std::map<std::string, double> get_joint_positions(const moveit::core::RobotState* self)
std::map<std::string, double> getJointPositions(const moveit::core::RobotState* self)
{
std::map<std::string, double> joint_positions;
const std::vector<std::string>& variable_name = self->getVariableNames();
Expand All @@ -93,15 +93,15 @@ std::map<std::string, double> get_joint_positions(const moveit::core::RobotState
return joint_positions;
}

void set_joint_positions(moveit::core::RobotState* self, std::map<std::string, double>& joint_positions)
void setJointPositions(moveit::core::RobotState* self, std::map<std::string, double>& joint_positions)
{
for (const auto& item : joint_positions)
{
self->setVariablePosition(item.first, item.second);
}
}

std::map<std::string, double> get_joint_velocities(const moveit::core::RobotState* self)
std::map<std::string, double> getJointVelocities(const moveit::core::RobotState* self)
{
std::map<std::string, double> joint_velocity;
const std::vector<std::string>& variable_name = self->getVariableNames();
Expand All @@ -112,15 +112,15 @@ std::map<std::string, double> get_joint_velocities(const moveit::core::RobotStat
return joint_velocity;
}

void set_joint_velocities(moveit::core::RobotState* self, std::map<std::string, double>& joint_velocities)
void setJointVelocities(moveit::core::RobotState* self, std::map<std::string, double>& joint_velocities)
{
for (const auto& item : joint_velocities)
{
self->setVariableVelocity(item.first, item.second);
}
}

std::map<std::string, double> get_joint_accelerations(const moveit::core::RobotState* self)
std::map<std::string, double> getJointAccelerations(const moveit::core::RobotState* self)
{
std::map<std::string, double> joint_acceleration;
const std::vector<std::string>& variable_name = self->getVariableNames();
Expand All @@ -131,15 +131,15 @@ std::map<std::string, double> get_joint_accelerations(const moveit::core::RobotS
return joint_acceleration;
}

void set_joint_accelerations(moveit::core::RobotState* self, std::map<std::string, double>& joint_accelerations)
void setJointAccelerations(moveit::core::RobotState* self, std::map<std::string, double>& joint_accelerations)
{
for (const auto& item : joint_accelerations)
{
self->setVariableAcceleration(item.first, item.second);
}
}

std::map<std::string, double> get_joint_efforts(const moveit::core::RobotState* self)
std::map<std::string, double> getJointEfforts(const moveit::core::RobotState* self)
{
std::map<std::string, double> joint_effort;
const std::vector<std::string>& variable_name = self->getVariableNames();
Expand All @@ -150,7 +150,7 @@ std::map<std::string, double> get_joint_efforts(const moveit::core::RobotState*
return joint_effort;
}

void set_joint_efforts(moveit::core::RobotState* self, std::map<std::string, double>& joint_efforts)
void setJointEfforts(moveit::core::RobotState* self, std::map<std::string, double>& joint_efforts)
{
for (const auto& item : joint_efforts)
{
Expand Down Expand Up @@ -327,17 +327,17 @@ void initRobotState(py::module& m)
)")

// Getting and setting joint model group positions, velocities, accelerations
.def_property("joint_positions", &moveit_py::bind_robot_state::get_joint_positions,
&moveit_py::bind_robot_state::set_joint_positions, py::return_value_policy::copy)
.def_property("joint_positions", &moveit_py::bind_robot_state::getJointPositions,
&moveit_py::bind_robot_state::setJointPositions, py::return_value_policy::copy)

.def_property("joint_velocities", &moveit_py::bind_robot_state::get_joint_velocities,
&moveit_py::bind_robot_state::set_joint_velocities, py::return_value_policy::copy)
.def_property("joint_velocities", &moveit_py::bind_robot_state::getJointVelocities,
&moveit_py::bind_robot_state::setJointVelocities, py::return_value_policy::copy)

.def_property("joint_accelerations", &moveit_py::bind_robot_state::get_joint_accelerations,
&moveit_py::bind_robot_state::set_joint_accelerations, py::return_value_policy::copy)
.def_property("joint_accelerations", &moveit_py::bind_robot_state::getJointAccelerations,
&moveit_py::bind_robot_state::setJointAccelerations, py::return_value_policy::copy)

.def_property("joint_efforts", &moveit_py::bind_robot_state::get_joint_efforts,
&moveit_py::bind_robot_state::set_joint_efforts, py::return_value_policy::copy)
.def_property("joint_efforts", &moveit_py::bind_robot_state::getJointEfforts,
&moveit_py::bind_robot_state::setJointEfforts, py::return_value_policy::copy)

.def("set_joint_group_positions",
py::overload_cast<const std::string&, const Eigen::VectorXd&>(
Expand Down
4 changes: 2 additions & 2 deletions moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ namespace moveit_py
namespace bind_moveit_cpp
{
std::shared_ptr<moveit_cpp::PlanningComponent>
get_planning_component(std::shared_ptr<moveit_cpp::MoveItCpp>& moveit_cpp_ptr, const std::string& planning_component)
getPlanningComponent(std::shared_ptr<moveit_cpp::MoveItCpp>& moveit_cpp_ptr, const std::string& planning_component)
{
return std::make_shared<moveit_cpp::PlanningComponent>(planning_component, moveit_cpp_ptr);
}
Expand Down Expand Up @@ -144,7 +144,7 @@ void initMoveitPy(py::module& m)
R"(
Execute a trajectory (planning group is inferred from robot trajectory object).
)")
.def("get_planning_component", &moveit_py::bind_moveit_cpp::get_planning_component,
.def("get_planning_component", &moveit_py::bind_moveit_cpp::getPlanningComponent,
py::arg("planning_component_name"), py::return_value_policy::take_ownership,
R"(
Creates a planning component instance.
Expand Down

0 comments on commit f85727b

Please sign in to comment.