diff --git a/src/iiwa_config/config/moveit/moveit_cpp.yaml b/src/iiwa_config/config/moveit/moveit_cpp.yaml
index d9bf720..13c34c4 100644
--- a/src/iiwa_config/config/moveit/moveit_cpp.yaml
+++ b/src/iiwa_config/config/moveit/moveit_cpp.yaml
@@ -13,7 +13,7 @@ planning_pipelines:
plan_request_params:
planning_attempts: 1
planning_pipeline: ompl
- max_velocity_scaling_factor: 1.0
+ max_velocity_scaling_factor: 0.5
max_acceleration_scaling_factor: 1.0
ompl_rrtc: # Namespace for individual plan request
diff --git a/src/iiwa_description/urdf/iiwa7.urdf b/src/iiwa_description/urdf/iiwa7.urdf
new file mode 100644
index 0000000..289d447
--- /dev/null
+++ b/src/iiwa_description/urdf/iiwa7.urdf
@@ -0,0 +1,295 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ webots_ros2_control::Ros2ControlSystem
+
+
+
+
+
+ 0
+
+
+
+
+
+ 0
+
+
+
+
+
+ 0
+
+
+
+
+
+ 0
+
+
+
+
+
+ 0
+
+
+
+
+
+ 0
+
+
+
+
+
+ 0
+
+
+
+
diff --git a/src/iiwa_description/urdf/joints.xacro b/src/iiwa_description/urdf/joints.xacro
index c2440cc..978b2f1 100644
--- a/src/iiwa_description/urdf/joints.xacro
+++ b/src/iiwa_description/urdf/joints.xacro
@@ -12,43 +12,50 @@
+ lower="-2.97" upper="2.97" effort="500"
+ velocity="1.71" damping="0.5"/>
+ lower="-2.09" upper="2.09" effort="500"
+ velocity="1.71" damping="0.5"/>
+ lower="-2.97" upper="2.97" effort="500"
+ velocity="1.75" damping="0.5"/>
+ lower="-2.09" upper="2.09" effort="500"
+ velocity="2.27" damping="0.5"/>
+ lower="-2.97" upper="2.97" effort="500"
+ velocity="2.44" damping="0.5"/>
+ lower="-2.09" upper="2.09" effort="500"
+ velocity="3.14" damping="0.5"/>
+ lower="-3.05" upper="3.05" effort="500"
+ velocity="3.14" damping="0.5"/>
diff --git a/src/iiwa_description/urdf/macros.xacro b/src/iiwa_description/urdf/macros.xacro
index ad86164..ec8f866 100644
--- a/src/iiwa_description/urdf/macros.xacro
+++ b/src/iiwa_description/urdf/macros.xacro
@@ -30,14 +30,15 @@
-
+
+
-
\ No newline at end of file
+
diff --git a/src/iiwa_description/urdf/params.xacro b/src/iiwa_description/urdf/params.xacro
index 5a5e7db..483b3ce 100644
--- a/src/iiwa_description/urdf/params.xacro
+++ b/src/iiwa_description/urdf/params.xacro
@@ -2,14 +2,14 @@
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
@@ -20,4 +20,4 @@
-
\ No newline at end of file
+
diff --git a/src/iiwa_planning/CMakeLists.txt b/src/iiwa_planning/CMakeLists.txt
index 03c64ec..4c65cc5 100644
--- a/src/iiwa_planning/CMakeLists.txt
+++ b/src/iiwa_planning/CMakeLists.txt
@@ -10,11 +10,32 @@ find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
+target_include_directories(motion_planning_cpp PUBLIC include)
+
+ament_target_dependencies(motion_planning_cpp
+ rclcpp
+ moveit_ros_planning_interface
+ moveit_core
+ moveit_ros_planning
+ moveit_msgs
+ geometry_msgs
+)
-include_directories(include)
ament_python_install_package(${PROJECT_NAME})
+install(TARGETS
+ motion_planning_cpp
+ DESTINATION lib/${PROJECT_NAME}
+)
+
install(PROGRAMS
# scripts/motion_planning_test.py
scripts/motion_planning.py
diff --git a/src/iiwa_planning/scripts/motion_planning.py b/src/iiwa_planning/scripts/motion_planning.py
index bd4cf0b..4a4ec04 100644
--- a/src/iiwa_planning/scripts/motion_planning.py
+++ b/src/iiwa_planning/scripts/motion_planning.py
@@ -14,9 +14,10 @@ class MotionPlaning(Node):
super().__init__("motion_planning_node")
self.moveit = MoveItPy(node_name="motion_planning_node")
- self.robot_arm = self.moveit.get_planning_component("iiwa_arm")
+ self.robot_arm: PlanningComponent = self.moveit.get_planning_component("iiwa_arm")
self.planning_scene_monitor = self.moveit.get_planning_scene_monitor()
+
self.get_logger().warn("***********************************************************")
self.get_logger().info("MotionPlanning node is ready...")
self.get_logger().warn("***********************************************************")
diff --git a/src/iiwa_planning/src/motion_planning_cpp.cpp b/src/iiwa_planning/src/motion_planning_cpp.cpp
new file mode 100644
index 0000000..330763c
--- /dev/null
+++ b/src/iiwa_planning/src/motion_planning_cpp.cpp
@@ -0,0 +1,58 @@
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+
+class MotionPlanning: public rclcpp::Node
+{
+ public:
+ MotionPlanning() : Node("motion_planning_node",
+ rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
+ {
+ RCLCPP_INFO(get_logger(), "***************");
+ RCLCPP_INFO(get_logger(), "MotionPlaning node created");
+ RCLCPP_INFO(get_logger(), "***************");
+
+ timer_ = create_wall_timer(
+ std::chrono::milliseconds(200),
+ std::bind(&MotionPlanning::runOnce, this)
+ );
+
+ }
+
+ private:
+ void runOnce()
+ {
+ timer_->cancel();
+
+ }
+
+ private:
+ rclcpp::TimerBase::SharedPtr timer_;
+
+ const std::string planning_group_ = "iiwa_arm";
+ const std::string base_frame_ = "base_link";
+ const std::string ee_link = "link_ee";
+
+
+};
+
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}