0
$\begingroup$

Rosanswers logo

My purpose: In the Move Group Python Interface section in the Moveit it tutorials(kinetic version), it shows we can plan a motion for a move group to a desired pose for the end-effector and excute it as shown is the snippet of code below: pose_goal = geometry_msgs.msg.Pose() pose_goal.orientation.w = 1.0 pose_goal.position.x = 0.4 pose_goal.position.y = 0.1 pose_goal.position.z = 0.4 group.set_pose_target(pose_goal)

In this example code available on moveit site, however, we can only plan one pose (instead of a number of successive poses which can form a smooth line) at a time. So I've got an idea that I could augmented this piece of code with a subscriber which listens to a ROS sensor sending poses to move group in this code then plan and execute those desired pose. Ideally, in this way, I can perform a smooth trajectory with all the poses together.

My first question is whether what I am planning to do feasible and practical?

If so, I've got some other questions while working on it. I am using a leap motion sensor as a publisher (publishing hand positions) in the ROS structure. As I said I would build a subscriber in the Move Group Python Interface code to keep receiving new poses then plan and execute in time. To get the job done: First,I created a package named leap_moveit with the following depedencies: catkin_create_pkg leap_moveit rospy rospack moveit_commander moveit_msgs geometry_msgs

Secondly,I put the modified Move Group Python Interface code(the main difference is there is a subscriber to obtain new desired poses in this code). The subscriber is: leap_sub = rospy.Subscriber('leapmotion/data', leapros, go_to_joint_state)

Thirdly,I updated CMakeLists.txt and package.xml accordingly and created a launch file in the launch directory of my leap_moveit pkg.

Finally, when I run roslaunch leap_moveit leapmoveit.launch to try out the code, I throws the following errors. Can anyone help?

mario@mario:~$ roslaunch leap_moveit leapmoveit.launch
... logging to /home/mario/.ros/log/fa712ea8-1ab7-11e9-a98a-94c6911255ab/roslaunch-mario-30026.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/mario/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://localhost:32869/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /
    leapmoveit_node (leap_moveit/move_group_python_interface_leap.py)

auto-starting new master
process[master]: started with pid [30043]
    ROS_MASTER_URI=http://localhost:11311

setting /run_id to fa712ea8-1ab7-11e9-a98a-94c6911255ab
process[rosout-1]: started with pid [30063]
started core service [/rosout]
process[leapmoveit_node-2]: started with pid [30069]
Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info
[ERROR] [1547771310.996982652]: Robot model parameter not found! Did you remap 'robot_description'?
Traceback (most recent call last):
      File "/home/mario/catkin_ws/src/leap_moveit/src/move_

group_python_interface_leap.py", line 223, in <module>
    main()
  File "/home/mario/catkin_ws/src/leap_moveit/src/move_group_python_interface_leap.py", line 202, in main
    tutorial = MoveGroupPythonIntefaceTutorial()
  File "/home/mario/catkin_ws/src/leap_moveit/src/move_group_python_interface_leap.py", line 57, in __init__
    robot = moveit_commander.RobotCommander()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/moveit_commander/robot.py", line 151, in __init__
    self._r = _moveit_robot_interface.RobotInterface(robot_description, ns)


   RuntimeError: RobotInterfacePython: invalid robot model
    [leapmoveit_node-2] process has died [pid 30069, exit code 1, cmd /home/

mario/catkin_ws/src/leap_moveit/src/move_group_python_interface_leap.py __name:=leapmoveit_node __log:=/home/mario/.ros/log/fa712ea8-1ab7-11e9-a98a-94c6911255ab/leapmoveit_node-2.log].
log file: /home/mario/.ros/log/fa712ea8-1ab7-11e9-a98a-94c6911255ab/leapmoveit_node-2*.log

Thank a million for your great support!!! And my python code in src in my leap_moveit package is shown below:

#!/usr/bin/env python



import sys
import copy
import rospy
import moveit_commander

import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list

###
from leap_motion.msg import leap
from leap_motion.msg import leapros

## END_SUB_TUTORIAL

    def all_close(goal, actual, tolerance):
  
          all_equal = True
          if type(goal) is list:
                for index in range(len(goal)):
                  if abs(actual[index] - goal[index]) > toler
        
        ance:
                return False
        
          elif type(goal) is geometry_msgs.msg.PoseStamped:
            return all_close(goal.pose, actual.pose, tolerance)
        
          elif type(goal) is geometry_msgs.msg.Pose:
        return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
    
      return True

class MoveGroupPythonIntefaceTutorial(object):
  """MoveGroupPythonIntefaceTutorial"""
  def __init__(self):
    super(MoveGroupPythonIntefaceTutorial, self).__init__()

    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_leap',
                    anonymous=True)'''

    
robot = moveit_commander.RobotCommander()

scene = moveit_commander.PlanningSceneInterface()

group_name = "manipulator" 
group = moveit_commander.MoveGroupCommander(group_name)

display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                               moveit_msgs.msg.DisplayTrajectory,
                                               queue_size=20)

leap_sub = rospy.Subscriber('leapmotion/data', leapros, go_to_joint_state)

# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names

###
self.leap_sub = leap_sub 
self.go_to_joint_state = go_to_joint_state 



 def go_to_pose_goal(self, msg):
    ## We can plan a motion for this group to a desired pose for the
    ## end-effector:
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = msg.palmpos.y/1000
pose_goal.position.y = msg.palmpos.x/1000
pose_goal.position.z = msg.palmpos.y/1000
group.set_pose_target(pose_goal)

## Now, we call the planner to compute the plan and execute it.
plan = group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
#group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
group.clear_pose_targets()

## END_SUB_TUTORIAL

  def execute_plan(self, plan):
    # Copy class variables to local variables to make the web tutorials more clear.
    # In practice, you should use the class variables directly unless you have a good
    # reason not to.
    group = self.group
  

def main():
  try:
    tutorial = MoveGroupPythonIntefaceTutorial()
    
    tutorial.go_to_pose_goal() 

  except rospy.ROSInterruptException:
    return
  except KeyboardInterrupt:
    return

    if __name__ == '__main__':
    ## First initialize `moveit_commander`_ and a `rospy`_ node:
      moveit_commander.roscpp_initialize(sys.argv)
      rospy.init_node('move_group_python_interface_leap',
                    anonymous=True)
  main()
  
  rospy.spin()

Originally posted by stevensu1838 on ROS Answers with karma: 27 on 2019-01-17

Post score: 1


Original comments

Comment by jayess on 2019-01-17:
Can you please update your question with all of the comments removed? Your code is unnecessarily long with them in and difficult to read

Comment by stevensu1838 on 2019-01-18:
Hi buddy, updated. Basically, what I've done is I added a subscriber in the Move Group Python Interface example code.

Comment by stevensu1838 on 2019-01-18:
But it doesn't work as expected. Is my idea of creating a subscriber wrong? Can you do me a favour?

Comment by vk on 2019-07-14:
hi did yo succeed in Create a subscriber and executing a continuous path?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Just to clarify are you trying to build a visual servoing system?

You mentioned that you want to joint several trajectories together into a single smooth movement. The main challenge with this is that as standard MoveiIt will project trajectories who's start and end velocities are zero, this mean that if you join them together the motion will 'stutter' as the arm stops and starts between trajectories.

This is a complex problem and there are a few different solutions out there. One is known as velocity control, here instead of planning paths a control law is defined which sets the 6 DOF velocity of the end effector based upon the location of the target. When an appropriate control law is set this will result in very smooth following of a moving target, however singularity avoidance is not as easy as it when using pre-planned trajectories.

Another option would be to plan each new trajectory from the current robot state including its position and velocity. This way each new plan knows the robot is in motion and will plan a trajectory which smoothly joins onto this motion. I personally don't know how to do this with MoveiIt, although I've love to find out if it's possible.

Regarding your specific question here, it appears to be a launch file problem. The MoveGroup by default tries to load a robot model (URDF) from a parameter called robot_description this parameter doesn't appear to be set. You'll also need other nodes (real or simulation) that can provide JointState messages and provide a trajectory action server.

Hope this helps.

Update:

You can use the jog_arm package to velocity control a robot, this package allows you to set the 6DOF velocity of the end effector as well as gracefully handling singularities for you (saves you a lot of headaches!)

You second question is a good one. It all comes down to the design of your control law, this function will take in target positions and produce desired EE velocities. Ideally as the EE gets close the target the velocities will reduce so it gradually slows down and comes to rest at the target. It's easy to overshoot if your controller is under-damped or not quite reach the target if it's over-damped. But you'll need to experiment with the real robot hardware to tune this correctly. You'll want this control loop running at least 10 times per second in my experience.


Originally posted by PeteBlackerThe3rd with karma: 9529 on 2019-01-18

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by stevensu1838 on 2019-01-18:
Hey buddy, thanks a million for your reply. I am very keen to know more about the velocity control method you mentioned above. Do you know where I can find more instructions about it? I am very interested in setting the 6 DOF velocity of the end effector based upon the location of the target.

Comment by stevensu1838 on 2019-01-18:
But one more question is if we only set the velocity of the end effector, How can we can control the trajectory precisely? like if I move my target 5 cm, how can I ensure the end effector moves 5 cm too? Thx

Comment by PeteBlackerThe3rd on 2019-01-18:
I've added answers to your questions now.

Comment by gvdhoorn on 2019-01-18:\

You can use the jog_arm package to velocity control a robot setup with MoveIt,

I'm not sure that is what jog_arm does: jog_arm allows you to jog (ie: manually control) a robot and works best with a velocity control interface. It has no direct relation with MoveIt, nor does it use ..

Comment by gvdhoorn on 2019-01-18:
.. any part of the MoveIt infrastructure to communicate with robots (or to be more precise: their drivers).

There is support for joint position controlled robots, but it's going to be sub-optimal at best.

Comment by PeteBlackerThe3rd on 2019-01-18:
My mistake. I've updated this now.

Comment by stevensu1838 on 2019-01-25:
Hi, I've looked at the jog arm pkg and I found a not well explained pkg call jog api there. Since you both are ROS experts, can any of you tell me how to use this jog api pkg?

Comment by stevensu1838 on 2019-01-25:
I've asked the creator of that pkg. He said I look use it to do what I want. But did say how to use his package. Can you give me a hand?

Comment by PeteBlackerThe3rd on 2019-01-25:
I'm confused. All you need is the jog_arm packaged I linked to originally. You don't need the jog api package.

$\endgroup$

Your Answer

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