ros-industrial / industrial_moveit Goto Github PK

It looks like the moveit move_group move method does not populate the robot start state in the planning request. Therefore STOMP check if it is populated and if not it exits and prints a warning where OMPL will plan fine. Not sure if this is really a STOMP issue or a moveit issue. Maybe if the start state is not provided it should get the current state of the robot?

Hi,

As far as I understand it, industrial_moveit can currently only be used if you have a jointstate goal.
However, this prevents the planner from exploiting any additional freedom in the robot movement, e.g. if only the target position is fixed.
Is there already a way to get this working, or would the code need to be adapted to take also cartesian goals (which is probably significant effort)?

Thx!
Broes

Hi, all. Now I want to control my abb industrial robot to move to a specified pose target by listening to a topic which publishes pose target with poseStamped message. So I firstly try to control my robot to move to a pose target which is given in my python code and it move to the specified pose target successfully.

However, when I want to move my abb_irb4600 to the received pose_target continually by subscribing to a topic and run the following python code, the terminal show "MoveItCommanderException: Error setting joint target. Is IK running?" What's wrong with my code? I am just a novice programmer. I will be very appreciated if who can help me fixing my python code.

The following first part of code is no error

#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg

from std_msgs.msg import String, Empty
from geometry_msgs.msg import PoseStamped

def abb_irb4600_60_205_move_group_python_interface():
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('abb_irb4600_60_205_move_group_python_interface',
                  anonymous=True)
  robot = moveit_commander.RobotCommander()
  scene = moveit_commander.PlanningSceneInterface()
  group = moveit_commander.MoveGroupCommander("abb_irb4600")
  display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                       moveit_msgs.msg.DisplayTrajectory, queue_size=20)

##  rospy.Subscriber('goal', PoseStamped, callback)
  rospy.sleep(1)
##  print "============ Starting"
##  print "============ Reference frame: %s" % group.get_planning_frame()
##  print "============ End effector: %s" % group.get_end_effector_link()
##  print "============ Robot Groups:"
  print robot.get_group_names()
##  print "============ Printing robot state"
  print robot.get_current_state()

  print "============ Generating plan 1"
  pose_target = geometry_msgs.msg.Pose()
  pose_target.orientation.w = 1.0
  pose_target.position.x = 1.2
  pose_target.position.y = -0.5
  pose_target.position.z = 0.5

  group.set_pose_target(pose_target)

  plan1 = group.plan()
  print "============ Waiting while RVIZ displays plan..."
  rospy.sleep(1)

  print "============ Visualizing plan"
  display_trajectory = moveit_msgs.msg.DisplayTrajectory()
  display_trajectory.trajectory_start = robot.get_current_state()
  display_trajectory.trajectory.append(plan1)
  display_trajectory_publisher.publish(display_trajectory);

  print "============ Waiting while plan is visualized (again)..."
  rospy.sleep(1)
  group.execute(plan1)
  group.clear_pose_targets()
  group.get_current_joint_values()    

if __name__=='__main__':
  try:
    abb_irb4600_60_205_move_group_python_interface()

  except rospy.ROSInterruptException:
    pass

This is the second part code with error. It can receive message from the topic but cannot execute the plan and then go to the pose target. The error say "Is IK running?" I do set the KDL as the solver. But why my robot can move in the first part code. They are the same solver.

#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from std_msgs.msg import String, Empty
from geometry_msgs.msg import PoseStamped

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("abb_irb4600")

def poseMessageReceived(pose):
  rospy.loginfo(rospy.get_caller_id() + 'I heard x=%f y=%f z=%f w=%f', pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.w)
##  pose_target = PoseStamped() 
  pose_target = pose   
  group.set_pose_target(pose_target)
  plan = group.plan(pose_target)
  group.execute(plan)
  group.clear_pose_targets()

def abb_irb4600_60_205_move_group_python_interface():
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('abb_irb4600_60_205_move_group_python_interface',
                  anonymous=True)  

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

  rospy.Subscriber('abb/goal', PoseStamped, poseMessageReceived)
  rospy.spin()

if __name__=='__main__':
  try:
    abb_irb4600_60_205_move_group_python_interface()

  except rospy.ROSInterruptException:
    pass

When I rosrun my pose_target talker node and rosrun the python code

andy@andy-zhaoyang-k42-80:~/catkin_ws$ roslaunch abb_irb4600_60_205_moveit_config abb_irb4600_60_205_planning_execution.launch sim:=false robot_ip:=172.16.5.128

andy@andy-zhaoyang-k42-80:~/catkin_ws$ rosrun abb_irb4600_test abb_irb4600_60_205_movgroup_python_interface_topic.py

andy@andy-zhaoyang-k42-80:~/catkin_ws$ rosrun abb_irb4600_test talker.py

It shows the following error

[INFO] [1525919142.818554]: /abb_irb4600_60_205_move_group_python_interface_14772_1525919132759I heard x=0.300000 y=-0.200000 z=0.500000 w=0.100000
[ERROR] [1525919143.021401]: bad callback: <function poseMessageReceived at 0x7f4dbf1479b0>
Traceback (most recent call last):
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/andy/catkin_ws/src/abb_irb4600_test/abb_irb4600_60_205/planning/scripts/abb_irb4600_60_205_move_group_python_interface_topic.py", line 22, in poseMessageReceived
    plan = group.plan(pose_target)
  File "/home/andy/catkin_ws/src/moveit/moveit_commander/src/moveit_commander/move_group.py", line 509, in plan
    self.set_joint_value_target(joints)
  File "/home/andy/catkin_ws/src/moveit/moveit_commander/src/moveit_commander/move_group.py", line 221, in set_joint_value_target
    raise MoveItCommanderException("Error setting joint target. Is IK running?")
MoveItCommanderException: Error setting joint target. Is IK running?

I noticed a difference in behavior between OMPL and STOMP and thought you all might find it interesting:

I was getting the Start joint pose is out of bounds error that some other issues allude to. Using OMPL with the same planning requests produced no issues.

For me, I was using the industrial_calibration library which includes a set of 6 "calibrate-able joints" for both the target and the camera I had on the end of a robot. Turned out the definition of these joints had a really small valid range, -1 to 1, and I had exceeded that limit.

So the moral is: Stomp checks the ENTIRE start state for validity and OMPL only checks the active move group.

Documentation is missing, at the very least the steps to run any existing demos should be documented along with instructions for how to load STOMP and CLIK into moveit. Furthermore, launch, unit tests and executables files in STOMP that are no longer part of the build should be removed as they make it confusing when one inspects the ros packages.

    error_message<<".\nSet or add the 'collision_detector' parameter to an allowed collision detector in the move_group.launch file"<<std::endl;
    throw std::runtime_error(error_message.str());
  }
}

state.collision_robot = planning_scene->getCollisionRobot();

state.collision_world = planning_scene->getCollisionWorld();

}

}

The object getCollisionRobot and getCollisionWorld no longer exist

The unit test in stomp core is currently failing when the trajectory is initialized with the MININUM_CONTROL_COST initialization method.

I'm trying to load multiple MoveIt planning plugins into a single process and one of the approaches I'm investigating is exploiting namespacing using ros::NodeHandle instances.

The OMPL plugin (moveit_planners_ompl) nicely uses the NodeHandle passed in in the PlanningManager::initialize(..) call (here) which makes things work almost out-of-the-box.

Trying to do the same with constrained_ik::CLIKPlannerManager does not seem to work properly, as CLIKPlannerManager::initialize(..) seems to only pass the NodeHandle on to the CartesianDynReconfigServer and the ManagerDynReconfigServer, but not to the CartesianPlanner and JointInterpolationPlanner instances that it creates. Combined with the fact that constrained_ik::Constrained_IK (used by CartesianPlanner) also creates its own NodeHandle, parameters are then always resolved / loaded from the private namespace of the node, which breaks any namespacing / isolation attempts.

The reason I'm posting this issue is to understand why this was implemented in this way, and to see whether it would be ok (and feasible) to restructure things in such a way that a single root NodeHandle instance (with possibly others in sub-namespaces) is used throughout the object hierarchy headed by CLIKPlannerManager.

I have problem when I install stomp planner (Moveit/Rviz) (indigo) no library loading in my package.
I found this error (process has died) as you see. anyone see this before to help me!!!

[move_group-4] process has died [pid 19540, exit code 127, cmd /opt/ros/indigo/lib/moveit_ros_move_group/move_group --debug __name:=move_group __log:=/home/bsg/.ros/log/4acda98a-5c08-11e9-aab8-4ccc6a381ce7/move_group-4.log].
log file: /home/bsg/.ros/log/4acda98a-5c08-11e9-aab8-4ccc6a381ce7/move_group-4*.log

Hello everyone,

I have tested the demo: roslaunch industrial_moveit_test_moveit_config demo.launch

I have cloned the repository : git clone https://github.com/ros-industrial/industrial_moveit.git and git clone https://github.com/ros-industrial/stomp_ros.git

When executing the Planning Command: Plan ➟ All Ok:

But when executing the Planning Command: Execute ➟ Fail :

The error in console is the following:

[ERROR] [1562513138.517676173]:
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'joint_1': expected: 1.1972, current: 0

Could indicate how to fix this error?

Environment:

• OS: Ubuntu 18.04
• ROS: Melodic
• Moveit : Melodic
• ROS Industrial Core: Melodic

Thanks in advance for your answer.

Hi all,
I'm trying out STOMP as per the tutorial here, but havent figured out how to set orientation constraints for the end effector so that it maintains a certain orientation throughout the trajectory. Is it possible to set this via yaml?

Hi everybody,
I am working with a KUKA KR240 L210 MED with a safe control system and would like to control it via the kuka_rsi_hw_interface. Starting the moveit_planning_execution.launch works and the robot can be started without problems. Also the visualization and path planning via Rviz and the execution of the movement works. My problem now is that the robot jerks randomly and does not move smoothly. At first I thought, there are too few waypoints passed and increased the number but there was no improvement. Also different planners from moveit don't lead to a clear improvement of the movement.
Has someone already had similar problems and could help me?

The industrial_moveit meta-package should be created. The meta-package should contain dependencies on all of the packages in the github repo.

What do you suppose it would take to adapt the constraint_ik to dual arm capabilities, with overlapping toros joints? I have past experience with this kind of project but am not sure if there are any mathematical limitations to the way constraints are currently being applied to the null space. I understand that this solver originally did support dual arms but it no longer does? @drchrislewis

The wiki content (generated from package xml) does not currently have a valid email address (i.e. click-able). It should have at least one. This can be mine as maintainer if nobody else wants their email address.

Result is this error no matter what starting joint pose:

[ERROR] [/move_group] [1491459052.767709473, 39.284000000]: STOMP Start joint pose is out of bounds
[ERROR] [/move_group] [1491459052.767820584, 39.284000000]: STOMP failed to get the start and goal positions

I've tried EVERY joint pose and even set everything to be "continuous" which SHOULD return "true" for all satisfiesBounds() checks. Yet still fails.

I am testing Stomp and I noticed that when I ask it to plan a movement to a goal state equal/similar to the start state it generates an unnecessary movement instead of remaining there. Here is a video of what I mean:

https://www.opentest.co/share/341626809aaf11e6b14b47baa7c14fe2

@jrgnicho Is there something in the config that I could modify to avoid that? or is this normal for stomp?

I'm testing the STOMP planner with an UR5 real robot. I replaced OMPL by STOMP and works fine in simulation mode, but when I try to move the real robot, it doesn't move because only send positions, not velocities and accelerations.

Now that STOMP is in its own repository, let's transfer all open issues to ros-industrial/stomp_ros for issues it makes sense for.

Description

When seeding STOMP with a valid trajectory (from RRT), STOMP fails to find a solution by reaching the max number of iterations.

Background

My use-case is path planning in a highly constrained collision environment relative to other users. Below is the configuration in use. Note the absence of the polynomial filter.

stomp/iiwa_1:
  group_name: iiwa_1
  optimization:
    num_timesteps: 20
    num_iterations: 50
    num_iterations_after_valid: 15
    num_rollouts: 10
    max_rollouts: 100 
    initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST
    control_cost_weight: 0.0
  task:
    noise_generator:
      - class: stomp_moveit/NormalDistributionSampling
        stddev: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
    cost_functions:
      - class: stomp_moveit/CollisionCheck
        check_intermediate_collisions: false
        kernel_window_percentage: 0.07
        collision_penalty: 1.0
        cost_weight: 1.0
        longest_valid_joint_move: 0.005
    noisy_filters:
      - class: stomp_moveit/JointLimits
        lock_start: True
        lock_goal: True 

Expected behavior

Based on the above settings, I would expect STOMP to always find a valid trajectory and reach at most 15 iterations.

Cause

https://github.com/ros-industrial/industrial_moveit/blob/kinetic-devel/stomp_moveit/src/stomp_planner.cpp#L429

The polynomial smoothing for seeded inputs ends up throwing the trajectory into a collision, and even though I'm not using the polynomial update filter, it can't always find its way back to a collision-free trajectory after a significant number of iterations. This behavior was why I removed the polynomial update filter in the first place.

When I do remove the polynomial filter on the seeded trajectory, I do get the expected behavior I want, but now I have a trade-off of STOMP running a very low number of iterations given the same amount of time

Obviously the polynomial smoothing has a huge impact on the running efficiency of the algorithm, but I don't have a good understanding of why. (I didn't see any reference to polynomial filtering in the STOMP white paper)

Proposed solution

I would suggest only applying the polynomial filter on the seeded trajectory if STOMP is configured to use the polynomial update filter. I can contribute a pull request to get this done. However, this doesn't reconcile the issue with poor run-time efficiency, so I will need some guidance on how to deal with that (if anything can be done at all)

EDIT : Disregard my comments on the run-time of using the polynomial filter on the seed trajectory. The poor run-time was due to setting the longest_valid_joint_move from 0.05 to 0.005, which obviously turned up the intermediate collision checking quite a bit. Classic user error of tweaking two things at once. STOMP is completely working as expected after commenting out the seed trajectory bit and resetting that value.

I followed the tutorial to add constrained_ik to my moveit_config and it works fine with the demo.launch. But when I want to use the ik_solver to my own package(like calling setFromIK function), error occurs and it said that

/home/xxx/Imitation/devel/lib/IKPlan/load_model: symbol lookup error: /home/xxx/Imitation/devel/lib//libconstrained_ik_plugin.so: undefined symbol: _ZN2tf12poseMsgToKDLERKN13geometry_msgs5Pose_ISaIvEEERN3KDL5FrameE

Can someone tells me how to do the right configuration in a new package when you want to use the constrained_ik_solver.

Just a silly typo in the repo description (silly for me to report it):

ROS-Industrial movit meta-package

I when I start stomp and do not move my robot stomp is able to fine nice plans. However if I move the robot it gives the following error:

[ERROR] [1493821272.310179796]: STOMP Start joint pose is out of bounds
[ERROR] [1493821272.310235237]: STOMP failed to get the start and goal positions

I search inside stomp_planner.cpp and this happens because it includes my joint wheel states in the planning. I don't know if I'm doing something wrong but this joints from the wheels do not belong to the planning group. I even defined them as passive in the robot srdf but it din't work out.

Is it stomp also take into account the planning of this joint or I am doing something wrong?

I'm wondering if we can load OMPL's planner, if STOMP fails to find a solution? In current tutorials, it seems like we can only specify one in the planning_pipeline configuration.

Thanks!

I'm trying to build it with melodic and I have found this building error:

 Building CXX object CMakeFiles/industrial_collision_detection_plugin.dir/src/industrial_collision_detection_plugin.cpp.o
In file included from /usr/ros/melodic/include/moveit/planning_scene/planning_scene.h:43,
                 from /usr/ros/melodic/include/moveit/collision_detection/collision_plugin.h:41,
                 from /srv/robotica/ros/src/ros-industrial/industrial_moveit/industrial_collision_detection/include/industrial_collision_detection/industrial_collision_detection_plugin.h:28,
                 from /srv/robotica/ros/src/ros-industrial/industrial_moveit/industrial_collision_detection/src/industrial_collision_detection_plugin.cpp:26:
/usr/ros/melodic/include/moveit/collision_detection/collision_detector_allocator.h: In instantiation of ‘const string& collision_detection::CollisionDetectorAllocatorTemplate<CollisionWorldType, CollisionRobotType, CollisionDetectorAllocatorType>::getName() const [with CollisionWorldType = collision_detection::CollisionWorldIndustrial; CollisionRobotType = collision_detection::CollisionRobotIndustrial; CollisionDetectorAllocatorType = collision_detection::CollisionDetectorAllocatorIndustrial; std::__cxx11::string = std::__cxx11::basic_string<char>]’:
/usr/ros/melodic/include/moveit/collision_detection/collision_detector_allocator.h:79:22:   required from here
/usr/ros/melodic/include/moveit/collision_detection/collision_detector_allocator.h:81:44: error: ‘NAME’ is not a member of ‘collision_detection::CollisionDetectorAllocatorIndustrial’
     return CollisionDetectorAllocatorType::NAME;
                                            ^~~~
make[2]: *** [CMakeFiles/industrial_collision_detection_plugin.dir/build.make:63: CMakeFiles/industrial_collision_detection_plugin.dir/src/industrial_collision_detection_plugin.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:73: CMakeFiles/industrial_collision_detection_plugin.dir/all] Error 2
make: *** [Makefile:141: all] Error 2

any idea?

A Travis-CI configuration file needs to be added.

Related to ros-industrial/ros_industrial_issues#57

https://github.com/ros-industrial/industrial_moveit/blob/melodic-devel/industrial_moveit_test_support/package.xml#L7

Any plans to release this repo for Kinetic any time soon?
This is currently blocking an urgent release of our cob-repositories, in particular the release of cob_moveit_bringup and thus cob_manipulation because we are depending on stomp_moveit and industrial_collision_detection (see https://github.com/ipa320/cob_manipulation/blob/kinetic_dev/cob_moveit_bringup/package.xml#L17-L26).

@ipa-bfb @ipa-fmw FYI

The constraint should drive position error in the tool frame. It should inherit from goal_position, which drives position error in the base frame (see goal_tool_orientation for reference).

Hi,

I am using industrial moveit for my robot and the STOMP algorithm is working when I am calling it from the move_group interface in moveit. However, I would like to provide my own initial trajectory for STOMP to optimize but I cannot find how to do it.

I have found the "solve" method in stomp.cpp that takes a "Eigen::MatrixXd& initial_parameters" as parameter and I suppose I need to call it but I did not find where I should do it.

Is there some documentation about giving an initial trajectory to the STOMP algorithm ?

Hello, all. I want to use STOMP with my UR5 but I cannot compile industrial_collision_detection. I've tried compiling it on many machines (virtual os etc.) and always received following error:
error: redefinition of ‘struct collision_detection::DistanceRequest’ struct DistanceRequest
and of course many more.

I thought that maybe it was problem with Boost 1.66, however even on clean Ubuntu 16.04 with Boost 1.58 it doesn't work. I would be grateful for any advises.

My setup:

• Ubuntu 16.04
• ROS Kinetic
• Packages for UR5 and Robotiq gripper (and many more ofc).

Dear all,
Dear @gavanderhoorn

we are working with ROS indigo right now using a Universal Robot UR10. I am wondering if there is the possibility to set a limit of the TCP speed. There are lot of applications where one needs to set a defined TCP speed, like 100 mm/s or other values.

Anyway, I just found the two scaling factors which are most like only regarding the joint speed:

      group.setMaxVelocityScalingFactor(0.25);
      group.setMaxAccelerationScalingFactor(0.25);

These scaling factors refer to the URDF files of a robot. In Universal Robot that means for example:
velocity="3.2"/
Which unit is that? Is it only representing the joint speed in point to point movement or can I transfer this also to Cartesian movements and therefore to the tool centre point (TCP) of the robot system?

Do you have any idea how I can solve the problem?

Best
matl

I tried STOMP using mimic joints, but STOMP can't plan path avoid obstacle.
To use 6dof robot as 4dof, I change 2 joints to mimic joint.
If I use robot(6dof) used not mimic, STOMP can plan avoid obstacle.

STOMP can do it if obviously there are no obstacles around but STOMP show error messages.

If theare are obstacles around, STOMP shows just these error messages and can't plan.

Your environment

• ROS Distro: [Kinetic]
• OS Version: Ubuntu 16.04
• Robot: mh180-120 (http://wiki.ros.org/motoman_mh_support)

My system is Ubuntu 14.04, the ROS distribution is indigo. Recently I am working with Fetch robot in gazebo simulation and I want to use stomp for better motion planning.

I install stomp from apt: apt-get ros-indigo-stomp-moveit .Then I modify the move_group.launch to use stomp as default motion planner.

When I use stomp, I found that it can't work well all the time. If I start a new empty world in gazebo with a Fetch robot, most of the time it can plan a trajectory to execute, but after a while, once it plan fails, it can't plan any motion anymore, even though the target poses it used to plan. And I have to restart the whole world instead of move_group.launch. Once it planed fail, it will print these error:

[ERROR] [1566302768.596134715, 42.778000000]: STOMP Start joint pose is out of bounds
[ERROR] [1566302768.596181439, 42.778000000]: STOMP failed to get the start and goal positions

Here is my python code to test stomp on Fetch robot, I wonder what's wrong with my code and how to fix the unstable situation. This code works fine with OMPL.

#!/usr/bin/env python
#-*- coding=utf-8 -*-

import rospy
import moveit_commander
from geometry_msgs.msg import Quaternion, Point, Pose, PoseStamped
from moveit_msgs.msg import MoveItErrorCodes
from std_msgs.msg import Header
from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest
from moveit_msgs.msg import RobotState
from sensor_msgs.msg import JointState

class IKer(object):
    def __init__(self):
        self.ik_client = rospy.ServiceProxy('compute_ik', GetPositionIK)

    def compute_ik(self, pose_stamped, joints_name=None, timeout=rospy.Duration(10)):
        """Computes inverse kinematics for the given pose.

        Args:
            pose_stamped: geometry_msgs/PoseStamped.
            timeout: rospy.Duration. How long to wait before giving up on the
                IK solution.
        """
        request = GetPositionIKRequest()
        request.ik_request.pose_stamped = pose_stamped
        request.ik_request.group_name = 'arm_with_torso'
        request.ik_request.timeout = timeout
        response = self.ik_client(request)
        # error_str = response.error_code.val)
        success = response.error_code.val == MoveItErrorCodes.SUCCESS
        if not success:
            return None
        joints_state = response.solution.joint_state
        joints_position = []
        if joints_name is not None:
            for index, name in enumerate(joints_state.name):
                if name in joints_name:
                    joints_position.append(joints_state.position[index])

            joints_state.name = joints_name
            joints_state.position = joints_position
        
        return joints_state

rospy.init_node("hand_up")

ik = IKer()
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm_with_torso")
arm.set_pose_reference_frame("base_link")
arm.clear_pose_targets()

points = [
    Point(0.609014743623, 0.129241553455, 0.786647925569),
    Point(0.609014743623, 0.029241553455, 0.986647925569),
    Point(0.709014743623, -0.129241553455, 0.786647925569)
]

pos = Point(0.609014743623, 0.229241553455, 0.686647925569)
qua = Quaternion(0,0,0,1)
pose = Pose(pos, qua)
ps = PoseStamped()
ps.pose = pose
ps.header.frame_id = "base_link"

robot_state = RobotState()
joint_state = JointState()
joint_state.header = Header()
for p in points:
    ps.pose.position = p
    ps.header.stamp = rospy.Time.now()
    
    joints_value = ik.compute_ik(ps, arm.get_joints())
    joints_value.header = Header()
    joints_value.header.stamp = rospy.Time.now()

    joint_state.header.stamp = rospy.Time.now()
    joint_state.name = arm.get_joints()
    joint_state.position = arm.get_current_joint_values()
    robot_state.joint_state = joint_state

    arm.set_start_state(robot_state)
    arm.set_joint_value_target(joints_value)
    arm.allow_replanning(True)
    arm.set_planning_time(1000)

    plan = arm.plan()
    if plan is not None:
        arm.execute(plan)

    arm.clear_pose_targets()

I am fairly new to ROS (melodic), and I am writing a C++ program to use the MoveIt! package. On building my package using catkin_make, I am getting below error:

In file included from /opt/ros/melodic/include/moveit/robot_model/joint_model_group.h:41:0,
                 from /opt/ros/melodic/include/moveit/robot_model/robot_model.h:47,
                 from /opt/ros/melodic/include/moveit/robot_model_loader/robot_model_loader.h:41,
                 from /home/anubhav1772/catkin_ws/src/robot_moveit_config/src/robot_manipulation/robot_state.cpp:5:
/opt/ros/melodic/include/moveit/robot_model/joint_model.h:47:10: fatal error: Eigen/Geometry: No such file or directory
 #include <Eigen/Geometry>
          ^~~~~~~~~~~~~~~~
compilation terminated.

After going through few documentation online, I found out that I forgot to add below lines in my CMakeLists.txt file:

find_package(Eigen REQUIRED)
include_directories(include  ${Boost_INCLUDE_DIR} ${EIGEN_INCLUDE_DIRS})

But now, I am getting a new error:

CMake Error at robot_moveit_config/CMakeLists.txt:14 (find_package):
  By not providing "FindEigen.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "Eigen", but
  CMake did not find one.

  Could not find a package configuration file provided by "Eigen" with any of
  the following names:

    EigenConfig.cmake
    eigen-config.cmake

  Add the installation prefix of "Eigen" to CMAKE_PREFIX_PATH or set
  "Eigen_DIR" to a directory containing one of the above files.  If "Eigen"
  provides a separate development package or SDK, be sure it has been
  installed.

I don't have know what to do next because in order to solve above error, everyone seems to add the same two lines(that I told earlier) in the CMakeLists.txt on all the discussion forums. Your guidance will be highly appreciated. Thank you so much!

Can I use stomp dynamics constraints in ros

I'm working on a project using ros kinetics that would benefit greatly from a planner capable of object avoidance. I'm not entirely sure where to start. As a first test, I am trying to modify the stomp_test_kr210_moveit_config demo.launch to use kinematics_solver: constrained_ik/ConstrainedIKPlugin as described here

Doing so results in rviz crashing with this error message:

ros.constrained_ik: Unable to find ros parameter: constrained_ik_solver/manipulator/constraints
ros.constrained_ik: BREAKPOINT HIT
	file = /home/mike/ws_industrial/src/industrial_moveit/constrained_ik/src/constrained_ik.cpp
	line=57

[rviz_ubuntu_64011_6174358474395917019-5] process has died [pid 64058, exit code -5, cmd /opt/ros/kinetic/lib/rviz/rviz -d 

This makes me think that I need to upload the constraints to the param server before this will work.
I'm not entirely sure of the correct way to go about doing this because running the examples in the constrained_ik/examples directory (both as roslaunch constrained_ik goal_tool_position_example.launch and roslaunch constrained_ik launch_example.launch example:=goal_tool_position_example use_yaml:=true) result in the same error.

I've also tried following the other available tutorial on using constraint_ik but it isn't super clear how to bridge the gap between groovy and kinetic.

Thanks in advance!

There are a number of error messages that the constrained ik unit test prints to the console as described in PR #88; However, they don't break the build and CI still passes. To reproduce just run

catkin run_tests constrained_ik --no-deps

The exception thrown by the IK solver cause moveit to freeze or crash.

(see https://github.com/ros-industrial/industrial_moveit/blob/master/constrained_ik/src/constrained_ik_plugin.cpp#L136)