0
$\begingroup$

This is my first time asking a question and I don't know what I need to provide to describe the problem as clearly as possible, in simple terms like the title I really don't understand why I can't use code for planning, although I can drag using rviz and use set_named_target(). But my kinematics solution keeps timing out:

[ INFO] [1706368911.564736276]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1706368911.565651288]: Planner configuration 'dof3' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1706368911.565778408]: dof3/dof3: Starting planning with 1 states already in datastructure
[ERROR] [1706368916.566395276]: dof3/dof3: Unable to sample any valid states for goal tree
[ INFO] [1706368916.566436409]: dof3/dof3: Created 1 states (1 start + 0 goal)
[ INFO] [1706368916.566456321]: No solution found after 5.000714 seconds
[ WARN] [1706368916.566472186]: Timed out
[ INFO] [1706368916.586480882]: Unable to solve the planning problem

This is my urdf:

<?xml version="1.0" ?>
 
<robot name="jhr_arm">
    <material name="silver">
        <color rgba="0.700 0.700 0.700 1.000" />
    </material>
    <transmission name="revote1_tran">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="revote1">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="revote1_actr">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="revote2_tran">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="revote2">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="revote2_actr">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>
<transmission name="revote3_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="revote3">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="revote3_actr">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>

</transmission>
<gazebo>
    <plugin filename="libgazebo_ros_control.so" name="control" />
    <robotNamespace>/</robotNamespace>
    <plugin name="gazebo_ros_control">
        <robotNamespace>/</robotNamespace>
    </plugin>
</gazebo>
<gazebo reference="base_link">
    <material>Gazebo/Silver</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <selfCollide>true</selfCollide>
    <gravity>true</gravity>
</gazebo>
<gazebo reference="arm_ser2_1_1">
    <material>Gazebo/Silver</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="mid_link_1_1">
    <material>Gazebo/Silver</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="arm_ser2_2_1">
    <material>Gazebo/Silver</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="arm_link2_1_1">
    <material>Gazebo/Silver</material>
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <selfCollide>true</selfCollide>
</gazebo>

<gazebo reference="arm_link1_1_1">
    <material>Gazebo/Silver</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="virtual_link1">
    <material>Gazebo/Silver</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="virtual_link2">
    <material>Gazebo/Silver</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="camera">
    <material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="camera">
    <sensor name="camera" type="depth">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
            <horizontal_fov>1.047197533</horizontal_fov>
            <image>
                <format>R8G8B8</format>
                <width>640</width>
                <height>480</height>
            </image>
            <clip>
                <near>0.1</near>
                <far>5</far>
            </clip>
        </camera>
        <plugin filename="libgazebo_ros_openni_kinect.so" name="kinect_camera_controller">
            <cameraName>camera</cameraName>
            <!--话题命名空间-->
            <alwaysOn>true</alwaysOn>
            <updateRate>20</updateRate>
            <imageTopicName>rgb/image_raw</imageTopicName>
            <!--图像信息采集话题-->
            <depthImageTopicName>depth/image_raw</depthImageTopicName>
            <!--深度信息采集话题-->
            <pointCloudTopicName>depth/points</pointCloudTopicName>
            <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
            <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
            <frameName>deep_link</frameName>
            <baseline>0.1</baseline>
            <distortion_k1>0.0</distortion_k1>
            <distortion_k2>0.0</distortion_k2>
            <distortion_k3>0.0</distortion_k3>
            <distortion_t1>0.0</distortion_t1>
            <distortion_t2>0.0</distortion_t2>
            <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
    </sensor>
</gazebo>
<link name="world" />
<link name="base_link">
    <inertial>
        <origin rpy="0 0 0" xyz="0.1925708416135842 0.04895775173916969 0.028775634617196247" />
        <mass value="1.4639942548947709" />
        <inertia ixx="0.000782" ixy="0.0" ixz="3e-06" iyy="0.000781" iyz="4e-06" izz="0.001161" />
    </inertial>
    <visual>
        <origin rpy="0 0 0" xyz="0 0 0" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/base_link.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="silver" />
    </visual>
    <collision>
        <origin rpy="0 0 0" xyz="0 0 0" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/base_link.stl" scale="0.001 0.001 0.001" />
        </geometry>
    </collision>
</link>
<joint name="fixworld" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0" />
    <parent link="world" />
    <child link="base_link" />
    <axis xyz="0.0 0.0 0.0" />
</joint>
<link name="arm_ser2_1_1">
    <inertial>
        <origin rpy="0 0 0" xyz="6.738867572769758e-06 7.018164940308935e-06 0.028378630691068656" />
        <mass value="0.7533061265197037" />
        <inertia ixx="0.000263" ixy="-0.0" ixz="-0.0" iyy="0.000264" iyz="1e-06" izz="0.00026" />
    </inertial>
    <visual>
        <origin rpy="0 0 0" xyz="-0.19244 0.351213 -0.0565" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/arm_ser2_1_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="silver" />
    </visual>
    <collision>
        <origin rpy="0 0 0" xyz="-0.19244 0.351213 -0.0565" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/arm_ser2_1_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
    </collision>
</link>
<link name="mid_link_1_1">
    <inertial>
        <origin rpy="0 0 0" xyz="0.01653247586516518 -2.5477330228840245e-07 0.01653340804519944" />
        <mass value="0.12929186299685888" />
        <inertia ixx="9.4e-05" ixy="-0.0" ixz="-1.9e-05" iyy="6.8e-05" iyz="-0.0" izz="8.3e-05" />
    </inertial>
    <visual>
        <origin rpy="0 0 0" xyz="-0.19244 0.351213 -0.111" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/mid_link_1_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="silver" />
    </visual>
    <collision>
        <origin rpy="0 0 0" xyz="-0.19244 0.351213 -0.111" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/mid_link_1_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
    </collision>
</link>
<link name="arm_ser2_2_1">
    <inertial>
        <origin rpy="0 0 0" xyz="-0.02839127101440475 -2.1175017652197425e-05 -9.445330417884312e-06" />
        <mass value="0.753889318956643" />
        <inertia ixx="0.000261" ixy="0.0" ixz="-0.0" iyy="0.000264" iyz="0.0" izz="0.000264" />
    </inertial>
    <visual>
        <origin rpy="0 0 0" xyz="-0.22044 0.351213 -0.143" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/arm_ser2_2_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="silver" />
    </visual>
    <collision>
        <origin rpy="0 0 0" xyz="-0.22044 0.351213 -0.143" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/arm_ser2_2_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
    </collision>
</link>
<link name="arm_link2_1_1">
    <inertial>
        <origin rpy="0 0 0" xyz="-0.01948680165762834 -0.1480865289037942 2.55351295663786e-15" />
        <mass value="0.46545276347264536" />
        <inertia ixx="0.004106" ixy="2.7e-05" ixz="-0.0" iyy="0.000228" iyz="0.0" izz="0.004104" />
    </inertial>
    <visual>
        <origin rpy="0 0 0" xyz="-0.16594 0.351213 -0.143" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/arm_link2_1_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="silver" />
    </visual>
    <collision>
        <origin rpy="0 0 0" xyz="-0.16594 0.351213 -0.143" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/arm_link2_1_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
    </collision>
</link>
<link name="arm_link1_1_1">
    <inertial>
        <origin rpy="0 0 0" xyz="7.212597603165061e-05 -0.19404771748015964 0.01942490210652898" />
        <mass value="1.8693807605710293" />
        <inertia ixx="0.028746" ixy="2.4e-05" ixz="-0.0" iyy="0.001387" iyz="-0.000183" izz="0.029188" />
    </inertial>
    <visual>
        <origin rpy="0 0 0" xyz="-0.19244 -0.048787 -0.0535" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/arm_link1_1_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="silver" />
    </visual>
    <collision>
        <origin rpy="0 0 0" xyz="-0.19244 -0.048787 -0.0535" />
        <geometry>
            <mesh filename="package://jhr_arm_description/meshes/arm_link1_1_1.stl" scale="0.001 0.001 0.001" />
        </geometry>
    </collision>
</link>
<joint name="fixed1" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 -0.4 0.003" />
    <parent link="arm_link1_1_1" />
    <child link="arm_ser2_1_1" />
</joint>
<joint name="revote2" type="continuous">
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0545" />
    <parent link="arm_ser2_1_1" />
    <child link="mid_link_1_1" />
    <axis xyz="0.0 -0.0 1.0" />
</joint>
<joint name="fixed2" type="fixed">
    <origin rpy="0 0 0" xyz="0.028 0.0 0.032" />
    <parent link="mid_link_1_1" />
    <child link="arm_ser2_2_1" />
</joint>
<joint name="revote3" type="revolute">
    <origin rpy="0 0 0" xyz="-0.0545 0.0 0.0" />
    <parent link="arm_ser2_2_1" />
    <child link="arm_link2_1_1" />
    <axis xyz="1.0 -0.0 -0.0" />
    <limit effort="100" lower="-0.698132" upper="0.698132" velocity="100" />
</joint>
<joint name="revote1" type="revolute">
    <dynamics damping="0.1" friction="0.5" />
    <origin rpy="0 0 0" xyz="0.19244 0.048787 0.0535" />
    <parent link="base_link" />
    <child link="arm_link1_1_1" />
    <axis xyz="0.0 -0.0 1.0" />
    <limit effort="100" lower="-1.570796" upper="1.570796" velocity="100" />
</joint>
<link name="camera">
    <visual>
        <geometry>
            <box size="0.12 0.12 0.12" />
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0" />
        <material name="red">
            <color rgba="1 0 0 0.5" />
        </material>
    </visual>
    <collision>
        <geometry>
            <box size="0.12 0.12 0.12" />
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0" />
    </collision>
    <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="1" />
        <inertia ixx="0.0024" ixy="0" ixz="0" iyy="0.00246" iyz="0" izz="0.0024" />
    </inertial>
</link>
<joint name="camera_fixed" type="fixed">
    <parent link="base_link" />
    <child link="camera" />
    <origin rpy="1.57 1.57 0 " xyz="0 0 3" />
</joint>

I just ran MoveIt's demo.launch. I wrote a code to try. I also tried other planning solutions, but they didn't work:

target_pose = PoseStamped()
target_pose.header.frame_id = "base_link"
target_pose.pose.position.x = target_xyz[0]
target_pose.pose.position.y = target_xyz[1]
target_pose.pose.position.z = target_xyz[2]
target_pose.pose.orientation.w=1.0 

I tried pasting the coordinates of rviz, including xyz and xyzw, so that the planning can be successful. But what I want to do now is to get the xyz coordinates of the object after identifying the object through the camera. How to make the robotic arm reach the specified position? If this cannot be solved, can someone tell me? What should I do to achieve this? Is there any relevant tutorials or information? Thank you very much.

$\endgroup$
1
  • $\begingroup$ please do not write a comment with additional information ... info belongs in the question ... edit the question and delete the comment $\endgroup$
    – jsotola
    Jan 27 at 18:39

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.