Unable to move goal gizmo in rviz MoveIt demo
I created a MoveIt config with the setup wizard from my URDF. Why trying out the demo.launch I can plan to random goal positions, but I cannot specify goal positions manually by moving the gizmos. With earlier versions of the URDF, I was only to able to rotate the gizmos along the wrist axis. How do I need to specify my arm such that I can select goals to plan to freely?
The robot arm is a SCARA version, here my URDF:
<?xml version="1.0"?>
<robot name="rrbot"
xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="pole_offset" value="0.2" />
<xacro:property name="pole_length" value="3" />
<xacro:property name="pole_radius" value="0.05" />
<xacro:property name="width1" value="0.1" />
<xacro:property name="width2" value="0.1" />
<xacro:property name="width3" value="0.1" />
<xacro:property name="length1" value="-0.3" />
<xacro:property name="length2" value="-0.7" />
<xacro:property name="length3" value="-0.7" />
<xacro:property name="overlap_1_2" value="0.1" />
<xacro:property name="overlap_2_3" value="0.1" />
<xacro:property name="pi_rad" value="3.14" />
<!-- world link -->
<link name="world"/>
<!-- base_link and its fixed joint -->
<joint name="joint_fix" type="fixed">
<parent link="world"/>
<child link="sbase_link"/>
</joint>
<link name="sbase_link">
<collision>
<origin xyz="0 0 ${pole_offset}" rpy="0 0 0"/>
<geometry>
<cylinder length="${pole_length}" radius="${pole_radius}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 ${pole_offset}" rpy="0 0 0"/>
<geometry>
<cylinder length="${pole_length}" radius="${pole_radius}"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 ${pole_offset}" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<xacro:macro name="box_arm" params="name parent width length *origin">
<joint name="${name}_rotation" type="revolute">
<!-- <origin xyz="0 ${pole_radius + y + overlap_1_2} ${-0.5 * y}" rpy="0 0 0"/> -->
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
<child link="${name}_link"/>
<axis xyz="0 0 1"/>
<limit effort="1000" velocity="1000" lower="${-0.5* pi_rad}" upper="${0.5*pi_rad}"/>
</joint>
<link name="${name}_link">
<collision>
<origin xyz="0 ${0.5 * length} ${-0.5 * width}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${length} ${width}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 ${0.5 * length} ${-0.5 * width}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${length} ${width}"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 ${0.5 * length} ${-0.5 * width}" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
</xacro:macro>
<xacro:macro name="scara_arm" params="name min_z max_z ...
the "gizmos" are called interactive markers :)
the IM may not work as a SCARA is typically a 4dof robot. The default KDL IK solver does not work very well with such kinematic configurations. As the IM relies on the IK solver working, you cannot drag it around like you'd do with a 6 dof robot.
You may need to select Allow Approximate Solutions in the MoveIt RViz planning plugin, or generate a custom IK plugin for your 4dof robot.
So MoveIt dosent solve robot with less than 6 DOF ? To do custom IK Plugin he has to do inverse kinematics from scratch also what about joint velocity ? Does he have to compute jacobian ?
please read carefully: KDL does not work with 4 dof robots very well.
I did not say anything about MoveIt. MoveIt doesn't care. It works with 1 dof to
N
dof robots. As long as you have a working IK solver it can use it (or actually: if you want to be able to specify Pose goals, you'll need an IK solver. For jointspace goals you would not need one).Or use something like ikfast.
MoveIt (or really: OMPL) does not do anything with velocities. I'm not sure what you're referring to here.
Sorry for getting it wrong. Thank you for the detailed explanation
There is no need to apologise. I just wanted to make sure you understood things correctly.