Creation of cl_moveit2z

smacc2_client_library/cl_moveit2z/CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.8)
+project(cl_moveit2z)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+
+find_package(smacc2 REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+
+#set(YAML_CPP_LIBRARIES yaml-cpp)
+
+set(dependencies
+moveit_ros_planning_interface
+tf2
+tf2_ros
+smacc2
+tf2_geometry_msgs
+yaml_cpp_vendor
+)
+
+include_directories(include)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/cl_moveit2z/cl_moveit2z.cpp
+  src/cl_moveit2z/common.cpp
+  src/cl_moveit2z/components/cp_trajectory_history.cpp
+  src/cl_moveit2z/components/cp_grasping_objects.cpp
+
+  src/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_end_effector.cpp
+  src/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_known_state.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_joints.cpp
+  src/cl_moveit2z/client_behaviors/cb_pouring_motion.cpp
+  src/cl_moveit2z/client_behaviors/cb_detach_object.cpp
+  src/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.cpp
+  src/cl_moveit2z/client_behaviors/cb_end_effector_rotate.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_named_target.cpp
+  src/cl_moveit2z/client_behaviors/cb_attach_object.cpp
+  src/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
+
+ament_target_dependencies(${PROJECT_NAME}
+                          ${dependencies})
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+ament_package()

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/cl_moveit2z.hpp

@@ -0,0 +1,140 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*****************************************************************************************************************
+ *
+ * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
+ *
+ *****************************************************************************************************************/
+
+#pragma once
+
+//#include <smacc2/smacc_client.h>
+//#include <smacc2/smacc_signal.h>
+#include <smacc2/smacc.hpp>
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace cl_moveit2z
+{
+template <typename TSource, typename TOrthogonal>
+struct EvMoveGroupMotionExecutionSucceded
+: sc::event<EvMoveGroupMotionExecutionSucceded<TSource, TOrthogonal>>
+{
+};
+
+template <typename TSource, typename TOrthogonal>
+struct EvMoveGroupMotionExecutionFailed
+: sc::event<EvMoveGroupMotionExecutionFailed<TSource, TOrthogonal>>
+{
+};
+
+/*
+moveit_msgs/MoveItErrorCodes
+----------------------------
+int32 SUCCESS=1
+int32 FAILURE=99999
+int32 PLANNING_FAILED=-1
+int32 INVALID_MOTION_PLAN=-2
+int32 MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE=-3
+int32 CONTROL_FAILED=-4
+int32 UNABLE_TO_AQUIRE_SENSOR_DATA=-5
+int32 TIMED_OUT=-6
+int32 PREEMPTED=-7
+int32 START_STATE_IN_COLLISION=-10
+int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-11
+int32 GOAL_IN_COLLISION=-12
+int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-13
+int32 GOAL_CONSTRAINTS_VIOLATED=-14
+int32 INVALID_GROUP_NAME=-15
+int32 INVALID_GOAL_CONSTRAINTS=-16
+int32 INVALID_ROBOT_STATE=-17
+int32 INVALID_LINK_NAME=-18
+int32 INVALID_OBJECT_NAME=-19
+int32 FRAME_TRANSFORM_FAILURE=-21
+int32 COLLISION_CHECKING_UNAVAILABLE=-22
+int32 ROBOT_STATE_STALE=-23
+int32 SENSOR_INFO_STALE=-24
+int32 NO_IK_SOLUTION=-31
+int32 val
+*/
+
+class ClMoveit2z : public smacc2::ISmaccClient
+{
+public:
+  // this structure contains the default move_group configuration for any arm motion through move_group
+  // the client behavior will override these default values in a copy of this object so that their changes
+  // are not persinstent. In the other hand, if you change this client configuration, the parameters will be persistent.
+  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> moveGroupClientInterface;
+
+  std::shared_ptr<moveit::planning_interface::PlanningSceneInterface> planningSceneInterface;
+
+  ClMoveit2z(std::string groupName);
+
+  ClMoveit2z(const moveit::planning_interface::MoveGroupInterface::Options & options);
+
+  virtual ~ClMoveit2z();
+
+  void onInitialize() override;
+
+  void postEventMotionExecutionSucceded();
+
+  void postEventMotionExecutionFailed();
+
+  template <typename TOrthogonal, typename TSourceObject>
+  void onOrthogonalAllocation()
+  {
+    postEventMotionExecutionSucceded_ = [=]()
+    {
+      this->onSucceded_();
+      this->postEvent<EvMoveGroupMotionExecutionSucceded<TSourceObject, TOrthogonal>>();
+    };
+
+    postEventMotionExecutionFailed_ = [=]()
+    {
+      this->onFailed_();
+      this->postEvent<EvMoveGroupMotionExecutionFailed<TSourceObject, TOrthogonal>>();
+    };
+  }
+
+  template <typename TCallback, typename T>
+  boost::signals2::connection onMotionExecutionSuccedded(TCallback callback, T * object)
+  {
+    return this->getStateMachine()->createSignalConnection(onSucceded_, callback, object);
+  }
+
+  template <typename TCallback, typename T>
+  boost::signals2::connection onMotionExecutionFailed(TCallback callback, T * object)
+  {
+    return this->getStateMachine()->createSignalConnection(onFailed_, callback, object);
+  }
+
+  const moveit::planning_interface::MoveGroupInterface::Options & getOptions() const;
+
+private:
+  std::function<void()> postEventMotionExecutionSucceded_;
+  std::function<void()> postEventMotionExecutionFailed_;
+
+  smacc2::SmaccSignal<void()> onSucceded_;
+  smacc2::SmaccSignal<void()> onFailed_;
+
+  // std::string groupName_;
+  moveit::planning_interface::MoveGroupInterface::Options options_;
+};
+}  // namespace cl_moveit2z

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors.hpp

@@ -0,0 +1,44 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*****************************************************************************************************************
+ *
+ * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
+ *
+ *****************************************************************************************************************/
+#pragma once
+
+// BASIC MANIPULATION BEHAVIORSz
+#include "client_behaviors/cb_move_cartesian_relative.hpp"
+#include "client_behaviors/cb_move_end_effector.hpp"
+#include "client_behaviors/cb_move_end_effector_relative.hpp"
+#include "client_behaviors/cb_move_joints.hpp"
+#include "client_behaviors/cb_move_known_state.hpp"
+#include "client_behaviors/cb_move_named_target.hpp"
+
+// ADVANCED MANIPULATION BEHAVIORS
+#include "client_behaviors/cb_circular_pivot_motion.hpp"
+#include "client_behaviors/cb_end_effector_rotate.hpp"
+#include "client_behaviors/cb_move_cartesian_relative2.hpp"
+#include "client_behaviors/cb_move_end_effector_trajectory.hpp"
+#include "client_behaviors/cb_pouring_motion.hpp"
+
+// HISTORY BASED BEHAVIRORS
+#include "client_behaviors/cb_execute_last_trajectory.hpp"
+#include "client_behaviors/cb_move_last_trajectory_initial_state.hpp"
+#include "client_behaviors/cb_undo_last_trajectory.hpp"
+
+// GRASPING BEHAVIORS
+#include "client_behaviors/cb_attach_object.hpp"
+#include "client_behaviors/cb_detach_object.hpp"

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_attach_object.hpp

@@ -0,0 +1,43 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*****************************************************************************************************************
+ *
+ * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
+ *
+ *****************************************************************************************************************/
+
+#pragma once
+
+#include <cl_moveit2z/cl_moveit2z.hpp>
+#include <smacc2/smacc.hpp>
+
+namespace cl_moveit2z
+{
+class CbAttachObject : public smacc2::SmaccAsyncClientBehavior
+{
+public:
+  CbAttachObject(std::string targetObjectName);
+
+  CbAttachObject();
+
+  virtual void onEntry() override;
+
+  virtual void onExit() override;
+
+  std::string targetObjectName_;
+
+private:
+};
+}  // namespace cl_moveit2z

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.hpp

@@ -0,0 +1,63 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*****************************************************************************************************************
+ *
+ * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
+ *
+ *****************************************************************************************************************/
+#pragma once
+
+#include <tf2/transform_datatypes.h>
+#include "cb_move_end_effector_trajectory.hpp"
+
+namespace cl_moveit2z
+{
+// executes a circular trajectory of the tipLink around one axis defined
+// by the z-axis of the plaenePivotPose
+class CbCircularPivotMotion : public CbMoveEndEffectorTrajectory
+{
+public:
+  std::optional<double> angularSpeed_rad_s_;
+  std::optional<double> linearSpeed_m_s_;
+
+  // if not specified it would be used the current robot pose
+  std::optional<geometry_msgs::msg::Pose> relativeInitialPose_;
+
+  CbCircularPivotMotion(std::optional<std::string> tipLink = std::nullopt);
+
+  CbCircularPivotMotion(
+    const geometry_msgs::msg::PoseStamped & planePivotPose, double deltaRadians,
+    std::optional<std::string> tipLink = std::nullopt);
+
+  CbCircularPivotMotion(
+    const geometry_msgs::msg::PoseStamped & planePivotPose,
+    const geometry_msgs::msg::Pose & relativeInitialPose, double deltaRadians,
+    std::optional<std::string> tipLink = std::nullopt);
+
+  virtual void generateTrajectory() override;
+
+  virtual void createMarkers() override;
+
+protected:
+  // the rotation axis (z-axis) and the center of rotation in the space
+  geometry_msgs::msg::PoseStamped planePivotPose_;
+
+  double deltaRadians_;
+
+private:
+  void computeCurrentEndEffectorPoseRelativeToPivot();
+};
+
+}  // namespace cl_moveit2z