0
$\begingroup$

Rosanswers logo

Hi everyone, first time posting.

My question relates to my attempt at creating a custom robot from scratch and trying to control it using MoveIt. I reverse engineered a model for an existing toy robot here. I also modify the arm to include encoders which are read by a Particle Photon micro and feed into my ROS controller node via RS232. I would post my URDF but i cant attach files yet, it took many hours to design the robot using SolidWorks and its URDF exporter (Note: pretty cool program)

I followed this example to get my arm working, I can send joint position targets using this command

~/catkin_ws$ rostopic pub /myrobot/joint_1/command std_msgs/Float64 -- -0.2

The above successfully drives joint 1 to the required angle.

/myrobot/launch/robot_position_controllers.launch

<?xml version="1.0"?>
<launch>
  <rosparam file="$(find myrobot)/config/hardware.yaml" command="load"/>
  <rosparam file="$(find myrobot)/config/ros_controllers.yaml" command="load"/>
  <rosparam file="$(find myrobot)/config/joint_limits.yaml" command="load"/>

  <param name="robot_description" textfile="$(find robotarm_individual)/urdf/robotarm_individual.urdf" />
  <node name="myrobot_node" pkg="myrobot" type="myrobot_node" output="screen" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
      args="joint_state_controller myrobot/joint_1 myrobot/joint_2 myrobot/joint_3 myrobot/joint_4 "/>
</launch>

/myrobot/config/hardward.yaml

myrobot:
  hardware_interface:
    loop_hz: 10 # hz
    joints:
      - jt1_joint
      - jt2_joint
      - jt3_joint
      - jt4_joint
      - jt5_joint

/myrobot/config/jointlimits.yaml

joint_limits:
  jt1_joint:
    has_position_limits: true
    min_position: -2.22
    max_position: 2.22
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_jerk_limits: true
    max_jerk: 100.0
    has_effort_limits: true
    max_effort: 1.0
  jt2_joint:
    has_position_limits: true
    min_position: -0.84
    max_position: 1.84
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_jerk_limits: true
    max_jerk: 100.0
    has_effort_limits: true
    max_effort: 1.0
  jt3_joint:
    has_position_limits: true
    min_position: -2.63
    max_position: 2.63
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_jerk_limits: true
    max_jerk: 100.0
    has_effort_limits: true
    max_effort: 1.0
  jt4_joint:
    has_position_limits: true
    min_position: -0.6
    max_position: 0.6
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_jerk_limits: true
    max_jerk: 100.0
    has_effort_limits: true
    max_effort: 1.0
  jt5_joint:
    has_position_limits: true
    min_position: 0
    max_position: 0.03
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_jerk_limits: true
    max_jerk: 100.0
    has_effort_limits: true
    max_effort: 1.0

/myrobot/config/ros_contollers.yaml

# Publish all joint states
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

controller_list:
[]
myrobot:
  joint_1:
    type: position_controllers/JointPositionController
    joint: jt1_joint
  
  joint_2:
    type: position_controllers/JointPositionController
    joint: jt2_joint
  
  joint_3:
    type: position_controllers/JointPositionController
    joint: jt3_joint
  
  joint_4:
    type: position_controllers/JointPositionController
    joint: jt4_joint

The problem is, from what i can gather, MoveIt requires a group of joints, not individual joints

Following this tutorial MoveIt setup assistant I created a MoveIt configuration. This tutorial asks to specify the Planning Group, which is created including the 4 joints related to the arm part, the gripper is joint5. A note about 4DOF, I found that if i add "position_only_ik: True" to the kinematics.yaml file I can use fake controller to plan and execute movements successfully, to a limited degree that is, but it works.

Is there a code example showing how to construct a group of joints? The rosroboticslearning.com/ros-control has them as individual. I also looked a https://www.slaterobotics.com/blog/5abd8a1ed4442a651de5cb5b/how-to-implement-ros_control-on-a-custom-robot and it appeared that this also has joints individually specified.

Also both of the above example didn't show how to create a matching CMakeLists.txt file. Not being familiar with C++ make files it took me ages to fill in the blanks on that one, but thats another story. I've included my example below, although this isn't the problem as my code compiles just fine

cmake_minimum_required(VERSION 2.8.3)
project(myrobot)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  genmsg
  controller_manager
)

## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES myrobot
  CATKIN_DEPENDS
    roscpp
    controller_manager
    hardware_interface
)

include_directories(
  include
    ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
add_library(${PROJECT_NAME}
  src/myrobot_hardware_interface.cpp
)

add_executable(
  ${PROJECT_NAME}_node src/myrobot_hardware_interface.cpp
)

target_link_libraries(
  ${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
)

add_dependencies(${PROJECT_NAME}_node src/myrobot_hardware_interface.cpp)

# Mark executables and/or libraries for installation
install(TARGETS ${PROJECT_NAME}_node
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

My computer is running Ubuntu 18.04.4 LTS, bionic and $env | grep ROS

ROS_ETC_DIR=/opt/ros/melodic/etc/ros
LPR_ROS=/home/zonared/ros
ROS_ROOT=/opt/ros/melodic/share/ros
ROS_MASTER_URI=http://localhost:11311
ROS_VERSION=1
ROS_PYTHON_VERSION=2
ROS_PACKAGE_PATH=/home/zonared/ws_moveit/src:/home/zonared/catkin_ws/src:/home/zonared/ros/ifm3d/share:/opt/ros/melodic/share
ROSLISP_PACKAGE_DIRECTORIES=/home/zonared/ws_moveit/devel/share/common-lisp:/home/zonared/catkin_ws/devel/share/common-lisp
SESSION_MANAGER=local/ROS-ThinkPad-W540:@/tmp/.ICE-unix/2367,unix/ROS-ThinkPad-W540:/tmp/.ICE-unix/2367
ROS_DISTRO=melodic

Again, am I missing something? Is there another step in the process here that can accept a group of joints from MoveIt and deliver these to individual joint controllers? If I can get myrobot to work with position_controllers/JointPositionController's, then I would like to change to Trajectory controllers of some sort, but i cant find a code example of how to do that yet, again another story. At the moment I just want to get my robot to move via MoveIt.

Any help or comments much appreciated in advance, thanks heaps.


Originally posted by Zonared on ROS Answers with karma: 48 on 2020-07-04

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

You'll want to add a JointTrajectoryController to your ros_control configuration.

Something like this should work:

pos_joint_traj_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - jt1_joint
    - jt2_joint
    - jt3_joint
    - jt4_joint
    - jt5_joint
  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    jt1_joint: {trajectory: 0.2, goal: 0.1}
    jt2_joint: {trajectory: 0.2, goal: 0.1}
    jt3_joint: {trajectory: 0.2, goal: 0.1}
    jt4_joint: {trajectory: 0.2, goal: 0.1}
    jt5_joint: {trajectory: 0.2, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate: 25
  action_monitor_rate: 10

Note: the constraints section could require some tweaking to get the values right for your robot.

In general being able to control individual joints is nice, but typically I don't really have a use-case for it.

To me a JointGroupPositionController would make more sense, if you're not interested in executing trajectories.


Originally posted by gvdhoorn with karma: 86574 on 2020-07-05

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Zonared on 2020-07-05:
Hi gvdhoorm, thanks heaps for your reply. I would love to add/make a JointTrajectoryController, but I can't find any code examples of how to do that. The examples i found only show position or effort controllers, one joint at a time :(

Comment by gvdhoorn on 2020-07-05:
That's not how ros_control works.

As soon as you have a hardware_interface implemented -- which you have, or otherwise you could not have used those individual position_controllers/JointPositionControllers -- you can use all the controllers available in ros_control.

Provided of course that you use the correct version (ie: only use position_controllers if your hardware_interface only exposes PositionJointInterfaces for your joints, etc).

I would love to add/make a JointTrajectoryController, but I can't find any code examples of how to do that

which would make sense, as there is nothing "special" to do.

Comment by Zonared on 2020-07-05:
Ok, thanks again for your info. So does it come down to configuration and settings then? How do I combine all my individual controllers into a group that Moveit can use?

Comment by gvdhoorn on 2020-07-05:
That's what I show in my answer.

You configure the JointTrajectoryController to be loaded onto your hardware_interface and then configure MoveIt to use that controller.

But to verify things work I would suggest to first play around a bit with the rqt_joint_trajectory_controller.

Comment by Zonared on 2020-07-05:
Right-o, I will have to sit down and configure your suggestion. Again, thanks heaps for your assistance. I'll report back as soon as I get a chance to play.

Comment by Zonared on 2020-07-06:
Thanks again for your help, you were indeed correct. It works as expected! I added this to the bottom of my ros_controllers.yaml file:

jc4dof:
  type: position_controllers/JointTrajectoryController
  joints:
    - jt1_joint
    - jt2_joint
    - jt3_joint
    - jt4_joint
  constraints:
    goal_time: 2.0
    stopped_velocity_tolerance: 0.1
    jt1_joint: {trajectory: 0.2, goal: 0.1}
    jt2_joint: {trajectory: 0.2, goal: 0.1}
    jt3_joint: {trajectory: 0.2, goal: 0.1}
    jt4_joint: {trajectory: 0.2, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate: 25
  action_monitor_rate: 10

I did have to modify by URDF to include a non zero valve for velocity="0.5", if this was zero rqt_joint_trajectory_controller produced the following (only include just in case some else comes across this) dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) ZeroDivisionError: float division by zero

Much appreciate

$\endgroup$

Your Answer

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