How to release the end effector from leveling.
Closed this issue · 2 comments
yo4hi6o commented
Hi.
Is this a hardware issue with MyCobot?
We are currently using a 6-axis robot arm (MyCobot) controlled by MoveIt and with the addition of a gripper. Moveit's automatic route generation cannot sample the valid state of the goal tree if error: RRTConnect: z = 0.15 or less. I want to grab something close to the ground. As you can see in this image, I think one of the factors is that the angle of the 4th axis does not change downward. After investigating, I think that the function to keep the position of the end effector horizontal is working. Is there a way to unlock it and use Move to bring the end effector closer to the ground? We have confirmed that the designated position is a position where the target can be achieved.
pick_and_placer.cpp
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char **argv) {
ros::init(argc, argv, "pickandplacer");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(2);
spinner.start();
moveit::planning_interface::MoveGroupInterface arm("mycobot_arm");
arm.setPoseReferenceFrame("base_link");
ROS_INFO("Moving to prepare pose");
geometry_msgs::PoseStamped pose1;
pose1.header.frame_id = "base_link";
pose1.pose.position.x = 0.0;
pose1.pose.position.y = 0.25;
pose1.pose.position.z = 0.15;
moveit::planning_interface::MoveItErrorCode ret;
ROS_INFO("move to WP1");
arm.setPoseTarget(pose1);
ret = arm.move();
if (!ret) {
ROS_WARN("Fail: %i", ret.val);
arm.setNamedTarget("vertical");
arm.move();
}
ros::Duration(1.5).sleep();
ros::shutdown();
return 0;
}
URDF
<?xml version="1.0" ?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
<xacro:property name="width" value=".2" />
<material name="dark blue">
<color rgba="0.1 0.1 0.9 1" />
</material>
<material name="gray">
<color rgba="0.8 0.8 0.8 1" />
</material>
<link name="world"/>
<link name="base_link"/>
<joint name="world_to_base_link" type="fixed">
<parent link="world"/>
<child link="base_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- holder joint & link -->
<joint name="base_bottom_joint" type="fixed">
<parent link="base_link" />
<child link="base_bottom_link" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="base_bottom_link">
<inertial>
<origin xyz="0 0 0" />
<mass value="3" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
<visual name="base_bottom">
<origin xyz="0 0 0.01" rpy="0 0 0" />
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
<geometry>
<box size="0.15 0.15 0.02" />
</geometry>
</visual>
<collision name="base_bottom">
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.15 0.15 0.02" />
</geometry>
</collision>
</link>
<!-- body joint & link -->
<joint name="body_joint" type="fixed">
<parent link="base_bottom_link" />
<child link="body_link" />
<origin xyz="0 0 0.01" rpy="0 0 0" />
</joint>
<link name="body_link">
<collision>
<origin xyz="0.0 0 0" rpy=" 0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/collision/link1.stl" />
</geometry>
</collision>
<visual>
<origin xyz="0.0 0 0 " rpy=" 0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/visual/link1.stl" />
</geometry>
<material name="dark blue" />
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.075" />
<mass value="5" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</link>
<!-- arm1 joint & link -->
<joint name="arm1_joint" type="revolute">
<parent link="body_link" />
<child link="arm1_link" />
<origin xyz="0 0 0.0706" rpy="0 0 0" />
<axis xyz="0 0 1" />
<limit effort="30" lower="-2.88" upper="2.88" velocity="1.5" />
</joint>
<link name="arm1_link">
<collision>
<origin xyz="0.0 0 0 " rpy=" 0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/collision/link2.stl" />
</geometry>
</collision>
<visual>
<origin xyz="0.0 0 0 " rpy=" 0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/visual/link2.stl" />
</geometry>
<material name="gray" />
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.075" />
<mass value="0.1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<!-- arm2 joint & link -->
<joint name="arm2_joint" type="revolute">
<parent link="arm1_link" />
<child link="arm2_link" />
<origin xyz="0.03256 0 0.05976" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit effort="30" lower="-2.88" upper="2.88" velocity="1.5" />
</joint>
<link name="arm2_link">
<collision>
<origin xyz="0.0 0 0 " rpy=" 0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/collision/link3.stl" />
</geometry>
</collision>
<visual>
<origin xyz="0.0 0 0 " rpy=" 0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/visual/link3.stl" />
</geometry>
<material name="gray" />
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.06" />
<mass value="0.1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<!-- arm3 joint & link -->
<joint name="arm3_joint" type="revolute">
<parent link="arm2_link" />
<child link="arm3_link" />
<origin xyz="0.00272 0 0.1104" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit effort="30" lower="-2.88" upper="2.88" velocity="1.5" />
</joint>
<link name="arm3_link">
<collision>
<origin xyz="0.0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/collision/link4.stl" />
</geometry>
</collision>
<visual>
<origin xyz="0.0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/visual/link4.stl" />
</geometry>
<material name="gray" />
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.06" />
<mass value="0.1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<!-- arm4 joint & link -->
<joint name="arm4_joint" type="revolute">
<parent link="arm3_link" />
<child link="arm4_link" />
<origin xyz="-0.00164 0 0.096" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit effort="30.0" lower="-2.88" upper="2.88" velocity="1.5" />
</joint>
<link name="arm4_link">
<collision>
<origin xyz="0.0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/collision/link5.stl" />
</geometry>
</collision>
<visual>
<origin xyz="0.0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/visual/link5.stl" />
</geometry>
<material name="gray" />
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.06" />
<mass value="0.1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<!-- arm5 joint & link -->
<joint name="arm5_joint" type="revolute">
<parent link="arm4_link" />
<child link="arm5_link" />
<origin xyz="0.0288 0 0.02976" rpy="0 0 0" />
<axis xyz="0 0 1" />
<limit effort="30" lower="-2.88" upper="2.88" velocity="1.5" />
</joint>
<link name="arm5_link">
<collision>
<origin xyz="0.0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/collision/link6.stl" />
</geometry>
</collision>
<visual>
<origin xyz="0.0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mycobot_description/meshes/stl/visual/link6.stl" />
</geometry>
<material name="gray" />
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.06" />
<mass value="0.1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<!-- arm6 joint & link -->
<joint name="arm6_joint" type="revolute">
<parent link="arm5_link" />
<child link="arm6_link" />
<origin xyz="0 0.0456 0.03792" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit effort="30" lower="-3.05" upper="3.05" velocity="1.5" />
</joint>
<link name="arm6_link">
<collision>
<origin xyz="0.0 0 0" rpy="1.57075 0 0" />
<geometry>
<cylinder length="0.005" radius="0.02" />
</geometry>
</collision>
<visual>
<origin xyz="0.0 0 0" rpy="1.57075 0 0" />
<geometry>
<cylinder length="0.005" radius="0.02" />
</geometry>
<material name="dark blue" />
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.06" />
<mass value="0.1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<!-- gripper joint & link -->
<joint name="gitpper_joint" type="fixed">
<axis xyz="0 0 0"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<parent link="arm6_link"/>
<child link="gripper"/>
<origin xyz= "0 0 0" rpy = "0 0 0 "/>
</joint>
<link name="gripper">
<visual>
<geometry>
<mesh filename="package://mycobot_description/meshes/dae/gripper.dae" />
</geometry>
<origin xyz = "0 0 0 " rpy = " 0 0 0"/>
</visual>
</link>
<transmission name="trans_arm1_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="arm1_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="arm1_joint_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_arm2_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="arm2_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="arm2_joint_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_arm3_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="arm3_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="arm3_joint_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_arm4_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="arm4_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="arm4_joint_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_arm5_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="arm5_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="arm5_joint_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_arm6_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="arm6_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="arm6_joint_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- camera joint & link -->
<link name="camera_link" />
<joint name="camera" type="fixed">
<parent link="world" />
<child link="camera_link" />
<origin xyz="0.0 0.3 0.4" rpy="0.3 3.14 0"/>
</joint>
<!-- camera holder joint & link -->
<xacro:include filename="$(find gazebo_sensors_tutorial)/robot/sensors/cam_holder.urdf.xacro"/>
<joint name="camera_holder_joint" type="fixed">
<parent link="base_link" />
<child link="camera_holder_link" />
<origin xyz="0 -0.50 0" rpy="1.5705 0 0"/>
</joint>
<link name="camera_holder_link" />
<xacro:cam_holder name="camera_holder" parent="base_link">
<origin xyz="0 -0.17 1.0" rpy="0 0 1.5705"/>
</xacro:cam_holder>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/</robotNamespace>
</plugin>
</gazebo>
anla-xu commented
@yo4hi6o It looks like the end effector is very close to the ground in the picture, doesn't it? Can you provide a more detailed description? I cannot understand your needs from your expression.
blcuwc commented
@yo4hi6o It is good that you confirmed the designed position is avaliable for the robot, and this shows that it is less possible to be a hardware issue of the robot.
According to the error log you provided RRTConnect: z = 0.15 or less and your code pose1.pose.position.z = 0.15;, it seems that there is something wrong with position z setting. Please try other values of position z setting, and see if it's possible to reach your goal. Otherwise, check the corresponding URDF file and exam the whole moving process.
Hope this helps.