TCPリンクを追加

crane_plus_description/urdf/crane_plus.urdf.xacro

@@ -47,13 +47,15 @@
   <xacro:property name="NAME_LINK_3" value="crane_plus_link3"/>
   <xacro:property name="NAME_LINK_4" value="crane_plus_link4"/>
   <xacro:property name="NAME_LINK_HAND" value="crane_plus_link_hand"/>
+  <xacro:property name="NAME_LINK_TCP" value="crane_plus_link_tcp"/>
 
   <xacro:property name="NAME_JOINT_BASE" value="crane_plus_joint_base"/>
   <xacro:property name="NAME_JOINT_1" value="crane_plus_joint1"/>
   <xacro:property name="NAME_JOINT_2" value="crane_plus_joint2"/>
   <xacro:property name="NAME_JOINT_3" value="crane_plus_joint3"/>
   <xacro:property name="NAME_JOINT_4" value="crane_plus_joint4"/>
   <xacro:property name="NAME_JOINT_HAND" value="crane_plus_joint_hand"/>
+  <xacro:property name="NAME_JOINT_TCP" value="crane_plus_joint_tcp"/>
 
   <xacro:property name="SERVO_HOME" value="${radians(150.0)}"/>
   <xacro:property name="JOINT_EFFORT_LIMIT" value="1.5"/>

crane_plus_description/urdf/crane_plus.xacro

@@ -224,5 +224,13 @@
       </inertial>
     </link>
 
+    <joint name="${NAME_JOINT_TCP}" type="fixed">
+      <origin xyz="0 0 0.121" rpy="0 0 0"/>
+      <parent link="${NAME_LINK_4}"/>
+      <child link="${NAME_LINK_TCP}"/>
+    </joint>
+
+    <link name="${NAME_LINK_TCP}"/>
+
   </xacro:macro>
 </robot>

crane_plus_examples/launch/camera_example.rviz

@@ -3,7 +3,8 @@ Panels:
     Help Height: 0
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
+      Expanded:
+        - /TF1/Frames1
       Splitter Ratio: 0.5
     Tree Height: 244
   - Class: rviz_common/Selection
@@ -144,6 +145,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          crane_plus_link_tcp:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -171,7 +176,7 @@ Visualization Manager:
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
-        Planning Group: arm
+        Planning Group: arm_tcp
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -266,6 +271,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          crane_plus_link_tcp:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -357,6 +366,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        crane_plus_link_tcp:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         world:
           Alpha: 1
           Show Axes: false
@@ -432,6 +445,8 @@ Visualization Manager:
           Value: false
         crane_plus_link_hand:
           Value: false
+        crane_plus_link_tcp:
+          Value: false
         world:
           Value: false
       Marker Scale: 1
@@ -461,6 +476,8 @@ Visualization Manager:
                     crane_plus_link4:
                       crane_plus_link_hand:
                         {}
+                      crane_plus_link_tcp:
+                        {}
       Update Interval: 0
       Value: true
     - Class: rviz_default_plugins/Image

crane_plus_examples/src/joint_values.cpp

@@ -41,7 +41,7 @@ int main(int argc, char ** argv)
   executor.add_node(move_group_node);
   std::thread([&executor]() {executor.spin();}).detach();
 
-  MoveGroupInterface move_group_arm(move_group_node, "arm");
+  MoveGroupInterface move_group_arm(move_group_node, "arm_tcp");
   move_group_arm.setMaxVelocityScalingFactor(1.0);  // Set 0.0 ~ 1.0
   move_group_arm.setMaxAccelerationScalingFactor(1.0);  // Set 0.0 ~ 1.0
 

crane_plus_examples/src/pick_and_place.cpp

@@ -46,7 +46,7 @@ int main(int argc, char ** argv)
   executor.add_node(move_group_gripper_node);
   std::thread([&executor]() {executor.spin();}).detach();
 
-  MoveGroupInterface move_group_arm(move_group_arm_node, "arm");
+  MoveGroupInterface move_group_arm(move_group_arm_node, "arm_tcp");
   move_group_arm.setMaxVelocityScalingFactor(1.0);  // Set 0.0 ~ 1.0
   move_group_arm.setMaxAccelerationScalingFactor(1.0);  // Set 0.0 ~ 1.0
 
@@ -80,19 +80,21 @@ int main(int argc, char ** argv)
   geometry_msgs::msg::Pose target_pose;
   tf2::Quaternion q;
   target_pose.position.x = 0.0;
-  target_pose.position.y = -0.09;
+  target_pose.position.y = -0.21;
   target_pose.position.z = 0.17;
   q.setRPY(to_radians(0), to_radians(90), to_radians(-90));
   target_pose.orientation = tf2::toMsg(q);
   move_group_arm.setPoseTarget(target_pose);
   move_group_arm.move();
 
+  target_pose.position.y = -0.09;
+  target_pose.position.z = 0.05;
   q.setRPY(to_radians(0), to_radians(180), to_radians(-90));
   target_pose.orientation = tf2::toMsg(q);
   move_group_arm.setPoseTarget(target_pose);
   move_group_arm.move();
 
-  target_pose.position.z = 0.14;
+  target_pose.position.z = 0.02;
   move_group_arm.setPoseTarget(target_pose);
   move_group_arm.move();
 
@@ -101,15 +103,15 @@ int main(int argc, char ** argv)
   move_group_gripper.setJointValueTarget(gripper_joint_values);
   move_group_gripper.move();
 
-  target_pose.position.z = 0.17;
+  target_pose.position.z = 0.05;
   move_group_arm.setPoseTarget(target_pose);
   move_group_arm.move();
 
   // ----- Placing Preparation -----
   move_group_arm.setNamedTarget("home");
   move_group_arm.move();
 
-  target_pose.position.x = 0.15;
+  target_pose.position.x = 0.27;
   target_pose.position.y = 0.0;
   target_pose.position.z = 0.06;
   q.setRPY(to_radians(0), to_radians(90), to_radians(0));

crane_plus_examples/src/pick_and_place_tf.cpp

@@ -47,7 +47,7 @@ class PickAndPlaceTf : public rclcpp::Node
   : Node("pick_and_place_tf_node")
   {
     using namespace std::placeholders;
-    move_group_arm_ = std::make_shared<MoveGroupInterface>(move_group_arm_node, "arm");
+    move_group_arm_ = std::make_shared<MoveGroupInterface>(move_group_arm_node, "arm_tcp");
     move_group_arm_->setMaxVelocityScalingFactor(1.0);
     move_group_arm_->setMaxAccelerationScalingFactor(1.0);
 
@@ -85,7 +85,7 @@ class PickAndPlaceTf : public rclcpp::Node
     move_group_arm_->setPathConstraints(constraints);
 
     // 待機姿勢
-    control_arm(0.0, 0.0, 0.17, 0, 0, 0);
+    control_arm(0.0, 0.0, 0.3, 0, 0, 0);
 
     tf_buffer_ =
       std::make_unique<tf2_ros::Buffer>(this->get_clock());
@@ -153,35 +153,32 @@ class PickAndPlaceTf : public rclcpp::Node
     double theta_deg = theta_rad * 180.0 / 3.1415926535;
 
     // 把持対象物に正対する
-    control_arm(0.0, 0.0, 0.17, 0, 90, theta_deg);
+    control_arm(0.0, 0.0, 0.3, 0, 0, theta_deg);
 
     // 掴みに行く
-    const double GRIPPER_OFFSET = 0.13;
-    double gripper_offset_x = GRIPPER_OFFSET * std::cos(theta_rad);
-    double gripper_offset_y = GRIPPER_OFFSET * std::sin(theta_rad);
-    if (!control_arm(x - gripper_offset_x, y - gripper_offset_y, 0.04, 0, 90, theta_deg)) {
+    if (!control_arm(x, y, 0.04, 0, 90, theta_deg)) {
       // アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る
-      control_arm(0.0, 0.0, 0.17, 0, 0, 0);
+      control_arm(0.0, 0.0, 0.3, 0, 0, 0);
       return;
     }
 
     // ハンドを閉じる
     control_gripper(GRIPPER_CLOSE);
 
     // 移動する
-    control_arm(0.0, 0.0, 0.17, 0, 90, 0);
+    control_arm(0.12, 0.0, 0.17, 0, 90, 0);
 
     // 下ろす
-    control_arm(0.0, -0.15, 0.05, 0, 90, -90);
+    control_arm(0.0, -0.27, 0.05, 0, 90, -90);
 
     // ハンドを開く
     control_gripper(GRIPPER_OPEN);
 
     // 少しだけハンドを持ち上げる
-    control_arm(0.0, -0.15, 0.10, 0, 90, -90);
+    control_arm(0.0, -0.27, 0.10, 0, 90, -90);
 
     // 待機姿勢に戻る
-    control_arm(0.0, 0.0, 0.17, 0, 0, 0);
+    control_arm(0.0, 0.0, 0.3, 0, 0, 0);
 
     // ハンドを閉じる
     control_gripper(GRIPPER_DEFAULT);

crane_plus_examples/src/pose_groupstate.cpp

@@ -34,7 +34,7 @@ int main(int argc, char ** argv)
   executor.add_node(move_group_arm_node);
   std::thread([&executor]() {executor.spin();}).detach();
 
-  MoveGroupInterface move_group_arm(move_group_arm_node, "arm");
+  MoveGroupInterface move_group_arm(move_group_arm_node, "arm_tcp");
   move_group_arm.setMaxVelocityScalingFactor(1.0);  // Set 0.0 ~ 1.0
   move_group_arm.setMaxAccelerationScalingFactor(1.0);  // Set 0.0 ~ 1.0
 

crane_plus_gazebo/worlds/table_with_aruco_cube.sdf

@@ -43,7 +43,7 @@
           model://aruco_cube_0
         </uri>
       </include>
-      <pose>0.2 0.05 1.05 0 0 0</pose>
+      <pose>0.2 0 1.05 0 0 0</pose>
     </model>
 
   </world>

crane_plus_gazebo/worlds/table_with_red_cube.sdf

@@ -44,7 +44,7 @@
           model://aruco_cube_0
         </uri>
       </include>
-      <pose>0.2 0.05 1.05 3.1415 0 0</pose>
+      <pose>0.2 0 1.05 3.1415 0 0</pose>
     </model>
 
   </world>

crane_plus_moveit_config/config/crane_plus.srdf

@@ -12,6 +12,9 @@
     <group name="arm">
         <chain base_link="crane_plus_base" tip_link="crane_plus_link4"/>
     </group>
+    <group name="arm_tcp">
+      <chain base_link="crane_plus_base" tip_link="crane_plus_link_tcp" />
+    </group>
     <group name="gripper">
         <link name="crane_plus_link_hand"/>
         <joint name="crane_plus_joint_hand"/>
@@ -29,8 +32,21 @@
         <joint name="crane_plus_joint3" value="-2.01"/>
         <joint name="crane_plus_joint4" value="-0.73"/>
     </group_state>
+    <group_state name="vertical" group="arm_tcp">
+        <joint name="crane_plus_joint1" value="0"/>
+        <joint name="crane_plus_joint2" value="0"/>
+        <joint name="crane_plus_joint3" value="0"/>
+        <joint name="crane_plus_joint4" value="0"/>
+    </group_state>
+    <group_state name="home" group="arm_tcp">
+        <joint name="crane_plus_joint1" value="0"/>
+        <joint name="crane_plus_joint2" value="-1.16"/>
+        <joint name="crane_plus_joint3" value="-2.01"/>
+        <joint name="crane_plus_joint4" value="-0.73"/>
+    </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="end_effector" parent_link="crane_plus_link4" group="gripper"/>
+    <end_effector name="end_effector" parent_link="crane_plus_link4" group="gripper" parent_group="arm"/>
+    <end_effector name="end_effector_tcp" parent_link="crane_plus_link_tcp" group="gripper" parent_group="arm_tcp"/>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="table" type="fixed" parent_frame="world" child_link="base_link"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->

crane_plus_moveit_config/config/kinematics.yaml

@@ -1,4 +1,9 @@
 arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
+
+arm_tcp:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005

crane_plus_moveit_config/config/moveit.rviz

@@ -17,7 +17,6 @@ Panels:
   - Class: rviz_common/Views
     Expanded:
       - /Current View1
-      - /Current View1/Focal Point1
     Name: Views
     Splitter Ratio: 0.5
 Visualization Manager:
@@ -106,6 +105,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          crane_plus_link_tcp:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -133,7 +136,7 @@ Visualization Manager:
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
-        Planning Group: arm
+        Planning Group: arm_tcp
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -190,6 +193,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          crane_plus_link_tcp:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           world:
             Alpha: 1
             Show Axes: false
@@ -243,6 +250,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        crane_plus_link_tcp:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         world:
           Alpha: 1
           Show Axes: false
@@ -306,7 +317,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 0.800000011920929
+      Distance: 0.8999999761581421
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1