Skip to content

Commit

Permalink
Fixed pre-commit relavat stuffs (#27)
Browse files Browse the repository at this point in the history 
Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
  • Loading branch information
@DarkusAlphaHydranoid @CihatAltiparmak
DarkusAlphaHydranoid and CihatAltiparmak authored Jul 26, 2024
1 parent d54937e commit ff5dc14
Show file tree
Hide file tree
Showing 4 changed files with 63 additions and 121 deletions.
8 changes: 2 additions & 6 deletions demo/src/pipeline_testbench_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -322,12 +322,8 @@ int main(int argc, char** argv)
RCLCPP_INFO(LOGGER, "Starting Pipeline Testbench example ...");
for (const auto& motion_plan_req : demo.getMotionPlanRequests())
{
demo.planAndVisualize(
{
{ "ompl", "RRTConnectkConfigDefault" },
{ "stomp", "stomp" },
{ "drake", ""} },
motion_plan_req);
demo.planAndVisualize({ { "ompl", "RRTConnectkConfigDefault" }, { "stomp", "stomp" }, { "drake", "" } },
motion_plan_req);
}

demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");
Expand Down
64 changes: 25 additions & 39 deletions include/ktopt_interface/ktopt_planning_context.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,59 +16,45 @@
namespace ktopt_interface
{
// declare all namespaces to be used
using drake::multibody::MultibodyPlant;
using drake::multibody::AddMultibodyPlantSceneGraph;
using drake::geometry::SceneGraph;
using drake::systems::DiagramBuilder;
using drake::multibody::AddMultibodyPlantSceneGraph;
using drake::multibody::MultibodyPlant;
using drake::multibody::PackageMap;
using drake::multibody::Parser;
using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization;
using drake::solvers::Solve;
using drake::systems::Diagram;
using drake::systems::Context;
using drake::multibody::PackageMap;
using Eigen::VectorXd;
using drake::systems::Diagram;
using drake::systems::DiagramBuilder;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Joints = std::vector<const moveit::core::JointModel*>;

class KTOptPlanningContext : public planning_interface::PlanningContext
{
public:
KTOptPlanningContext(
const std::string& name,
const std::string& group_name,
const ktopt_interface::Params& params);

void solve(
planning_interface::MotionPlanResponse& res) override;
void solve(
planning_interface::MotionPlanDetailedResponse& res) override;
KTOptPlanningContext(const std::string& name, const std::string& group_name, const ktopt_interface::Params& params);

bool terminate() override;
void clear() override;
void solve(planning_interface::MotionPlanResponse& res) override;
void solve(planning_interface::MotionPlanDetailedResponse& res) override;

void setRobotDescription(std::string robot_description);
VectorXd toDrakePositions(
const moveit::core::RobotState& state,
const Joints& joints);
void setJointPositions(
const VectorXd& values,
const Joints& joints,
moveit::core::RobotState& state);
void setJointVelocities(
const VectorXd& values,
const Joints& joints,
moveit::core::RobotState& state);
bool terminate() override;
void clear() override;

void setRobotDescription(std::string robot_description);
VectorXd toDrakePositions(const moveit::core::RobotState& state, const Joints& joints);
void setJointPositions(const VectorXd& values, const Joints& joints, moveit::core::RobotState& state);
void setJointVelocities(const VectorXd& values, const Joints& joints, moveit::core::RobotState& state);

private:
const ktopt_interface::Params params_;
std::string robot_description_;

// drake related variables
SceneGraph<double>* scene_graph_{};
MultibodyPlant<double>* plant_{};
std::unique_ptr<Context<double>> diagram_context_;
Context<double>* plant_context_;
VectorXd nominal_q_;
const ktopt_interface::Params params_;
std::string robot_description_;

// drake related variables
SceneGraph<double>* scene_graph_{};
MultibodyPlant<double>* plant_{};
std::unique_ptr<Context<double>> diagram_context_;
Context<double>* plant_context_;
VectorXd nominal_q_;
};
} // namespace ktopt_interface
} // namespace ktopt_interface
22 changes: 11 additions & 11 deletions src/ktopt_planner_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,24 +31,24 @@ class KTOptPlannerManager : public planning_interface::PlannerManager
node_ = node;
param_listener_ = std::make_shared<ktopt_interface::ParamListener>(node, parameter_namespace);


// set QoS to transient local to get messages that have already been published
// (if robot state publisher starts before planner)
// set QoS to transient local to get messages that have already been published
// (if robot state publisher starts before planner)
robot_description_subscriber_ = node_->create_subscription<std_msgs::msg::String>(
"robot_description", rclcpp::QoS(1).transient_local(), [this](const std_msgs::msg::String::SharedPtr msg){
if (robot_description_.empty())
{
robot_description_ = msg->data;
RCLCPP_INFO(getLogger(), "Robot description set");
}
});
"robot_description", rclcpp::QoS(1).transient_local(), [this](const std_msgs::msg::String::SharedPtr msg) {
if (robot_description_.empty())
{
robot_description_ = msg->data;
RCLCPP_INFO(getLogger(), "Robot description set");
}
});
RCLCPP_INFO(getLogger(), "KTOpt planner manager initialized!");
return true;
}

bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override
{
if(robot_description_.empty()){
if (robot_description_.empty())
{
RCLCPP_ERROR(getLogger(), "Robot description is empty, do you have a robot state publisher running?");
return false;
}
Expand Down
90 changes: 25 additions & 65 deletions src/ktopt_planning_context.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,8 @@ rclcpp::Logger getLogger()
}
} // namespace

KTOptPlanningContext::KTOptPlanningContext(
const std::string& name,
const std::string& group_name,
const ktopt_interface::Params& params)
KTOptPlanningContext::KTOptPlanningContext(const std::string& name, const std::string& group_name,
const ktopt_interface::Params& params)
: planning_interface::PlanningContext(name, group_name), params_(params)
{
// Do some drake initialization may be
Expand All @@ -33,30 +31,23 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse&

void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
{
RCLCPP_INFO(getLogger(),
"Setting up and solving optimization problem ..."
);
RCLCPP_INFO(getLogger(), "Setting up and solving optimization problem ...");
// preliminary house keeping
const auto time_start = std::chrono::steady_clock::now();
res.planner_id = std::string("ktopt");
res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;

// Retrieve motion plan request
const auto& req = getMotionPlanRequest();
const moveit::core::RobotState start_state(
*getPlanningScene()->getCurrentStateUpdated(
req.start_state));
const auto group = getPlanningScene()->getRobotModel()->getJointModelGroup(
getGroupName());
RCLCPP_INFO_STREAM(getLogger(),
"Planning for group"
<< getGroupName());
const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state));
const auto group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName());
RCLCPP_INFO_STREAM(getLogger(), "Planning for group" << getGroupName());
const auto& joints = group->getActiveJointModels();

// q represents the complete state (joint positions and velocities)
const auto q_p = toDrakePositions(start_state, joints);
VectorXd q_v = VectorXd::Zero(joints.size());
VectorXd q = VectorXd::Zero(2*joints.size());
VectorXd q = VectorXd::Zero(2 * joints.size());
q << q_p;
q << q_v;

Expand All @@ -74,9 +65,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
}

// compile into a Kinematic Trajectory Optimization problem
auto trajopt = KinematicTrajectoryOptimization(
plant_->num_positions(),
params_.control_points);
auto trajopt = KinematicTrajectoryOptimization(plant_->num_positions(), params_.control_points);
auto& prog = trajopt.get_mutable_prog();

// Costs
Expand All @@ -92,27 +81,11 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)

// Constraints
// Add constraints on start joint configuration and velocity
trajopt.AddPathPositionConstraint(
toDrakePositions(start_state, joints),
toDrakePositions(start_state, joints),
0.0
);
trajopt.AddPathVelocityConstraint(
VectorXd::Zero(joints.size()),
VectorXd::Zero(joints.size()),
0.0
);
trajopt.AddPathPositionConstraint(toDrakePositions(start_state, joints), toDrakePositions(start_state, joints), 0.0);
trajopt.AddPathVelocityConstraint(VectorXd::Zero(joints.size()), VectorXd::Zero(joints.size()), 0.0);
// Add constraint on end joint configuration and velocity
trajopt.AddPathPositionConstraint(
toDrakePositions(goal_state, joints),
toDrakePositions(goal_state, joints),
1.0
);
trajopt.AddPathVelocityConstraint(
VectorXd::Zero(joints.size()),
VectorXd::Zero(joints.size()),
1.0
);
trajopt.AddPathPositionConstraint(toDrakePositions(goal_state, joints), toDrakePositions(goal_state, joints), 1.0);
trajopt.AddPathVelocityConstraint(VectorXd::Zero(joints.size()), VectorXd::Zero(joints.size()), 1.0);
// TODO Add constraints on joint position/velocity/acceleration
// trajopt.AddPositionBounds(
// plant_->GetPositionLowerLimits(),
Expand Down Expand Up @@ -141,12 +114,10 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
auto traj = trajopt.ReconstructTrajectory(result);
const size_t num_pts = params_.trajectory_res; // TODO: should be sample time based instead
const auto time_step = traj.end_time() / static_cast<double>(num_pts - 1);
res.trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(
start_state.getRobotModel(),
group
);
res.trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(start_state.getRobotModel(), group);
res.trajectory->clear();
const auto& active_joints = res.trajectory->getGroup() ? res.trajectory->getGroup()->getActiveJointModels() : res.trajectory->getRobotModel()->getActiveJointModels();
const auto& active_joints = res.trajectory->getGroup() ? res.trajectory->getGroup()->getActiveJointModels() :
res.trajectory->getRobotModel()->getActiveJointModels();

// sanity check
assert(traj.rows() == active_joints.size());
Expand All @@ -165,8 +136,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)

bool KTOptPlanningContext::terminate()
{
RCLCPP_ERROR(getLogger(),
"KTOptPlanningContext::terminate() is not implemented!");
RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::terminate() is not implemented!");
return true;
}

Expand All @@ -183,9 +153,8 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description)
// auto robot_instance = Parser(plant_, scene_graph_).AddModelsFromString(robot_description_, ".urdf");

// HACK: For now loading directly from drake's package map
const char* ModelUrl =
"package://drake_models/franka_description/"
"urdf/panda_arm.urdf";
const char* ModelUrl = "package://drake_models/franka_description/"
"urdf/panda_arm.urdf";
const std::string urdf = PackageMap{}.ResolveUrl(ModelUrl);
auto robot_instance = Parser(plant_, scene_graph_).AddModels(urdf);
plant_->WeldFrames(plant_->world_frame(), plant_->GetFrameByName("panda_link0"));
Expand All @@ -197,15 +166,11 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description)
// diagram
auto diagram_ = builder.Build();
diagram_context_ = diagram_->CreateDefaultContext();
plant_context_ = &diagram_->GetMutableSubsystemContext(
*plant_,
diagram_context_.get());
plant_context_ = &diagram_->GetMutableSubsystemContext(*plant_, diagram_context_.get());
nominal_q_ = plant_->GetPositions(*plant_context_);
}

VectorXd KTOptPlanningContext::toDrakePositions(
const moveit::core::RobotState& state,
const Joints& joints)
VectorXd KTOptPlanningContext::toDrakePositions(const moveit::core::RobotState& state, const Joints& joints)
{
VectorXd q = VectorXd::Zero(joints.size());
for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index)
Expand All @@ -215,10 +180,8 @@ VectorXd KTOptPlanningContext::toDrakePositions(
return q;
}

void KTOptPlanningContext::setJointPositions(
const VectorXd& values,
const Joints& joints,
moveit::core::RobotState& state)
void KTOptPlanningContext::setJointPositions(const VectorXd& values, const Joints& joints,
moveit::core::RobotState& state)
{
for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index)
{
Expand All @@ -227,10 +190,8 @@ void KTOptPlanningContext::setJointPositions(
return;
}

void KTOptPlanningContext::setJointVelocities(
const VectorXd& values,
const Joints& joints,
moveit::core::RobotState& state)
void KTOptPlanningContext::setJointVelocities(const VectorXd& values, const Joints& joints,
moveit::core::RobotState& state)
{
for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index)
{
Expand All @@ -241,8 +202,7 @@ void KTOptPlanningContext::setJointVelocities(

void KTOptPlanningContext::clear()
{
RCLCPP_ERROR(getLogger(),
"KTOptPlanningContext::clear() is not implemented!");
RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::clear() is not implemented!");
// do something
}
} // namespace ktopt_interface

0 comments on commit ff5dc14

Please sign in to comment.