Stomp planner #631

bbrito · 2017-03-12T12:51:35Z

Stomp planner configuration added for raw3-1. Launch files too.

merge together with ipa320/cob_manipulation#104

floweisshardt

no launch files in cob_hardware https://github.com/ipa320/cob_robots/pull/631/files#diff-22447eacd575bff1ed87005f7ac42b32 (this is also in cob4-5 directory which is certainly not correct.)

bbrito · 2017-03-12T21:35:10Z

Which launch files are you referring to?

floweisshardt · 2017-03-12T21:57:03Z

renamed/moved one (probably not by purpose): cob_bringup/components/legacy_schunk_powercube.launch → cob_hardware_config/cob4-5/config/legacy_schunk_powercube.launch

fmessmer

Please review your PR for formal correctness before we can review the actual feature:

do not change indentation scheme
revert moving files
reference/link related PRs

fmessmer · 2017-03-13T07:38:16Z

cob_bringup/robots/cob4-5.xml

@@ -131,6 +132,7 @@
 			<arg name="component_name" value="arm_left"/>
 			<arg name="can_device" value="can3"/>
 			<arg name="sim" value="$(arg sim)"/>
+			<arg name="cartesian_control" value="$(arg cartesian_control)"/>


if we add this, then we add it for both arms...
however, adding it currently gives us load problems on the real hardware...
you could pass the argument only in case sim is active:
<arg if=$(arg sim) name="cartesian_control" value="$(arg cartesian_control)"/>

fmessmer · 2017-03-13T07:39:05Z

cob_bringup/robots/cob4-5.xml

@@ -11,6 +11,7 @@
 	<arg name="cob4-5-h1" default="localhost"/>
 	<arg name="env-script" default="$(find cob_bringup)/env.sh"/>
 	<arg name="sim" default="false"/>
+	<arg name="cartesian_control" default="false"/>


remove this as you cannot call a xml directly thus you cannot pass a launch argument..

fmessmer · 2017-03-13T07:40:14Z

cob_bringup/robots/raw3-1.xml

@@ -58,7 +59,7 @@
 			<arg name="robot" value="$(arg robot)"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
-		<include file="$(find cob_bringup)/components/legacy_schunk_powercube.launch">
+		<include file="$(find cob_bringup)/components/canopen_generic.launch">


this won't work without proper configuration/yaml files...I guess this hasn't been tested on HW...

The problem is that "someone" deleted legacy_schunk_powercube.launch so now the software that we have in the main repository is wrong fro raw3-1 and cob3-6. Anyway I will put this back as it was...

fmessmer · 2017-03-13T07:41:22Z

cob_hardware_config/raw3-1/config/arm_controller.yaml

+     - arm_wrist_3_joint
+  required_drive_mode: 1
+
+arm_1_joint_position_controller:


convention is to use <joint_name>_position_controller....-> arm_shoulder_pan_joint_position_controller
same for the other controllers...

please respect the convention!

fmessmer · 2017-03-13T07:42:45Z

cob_hardware_config/raw3-1/config/arm_controller.yaml

+  required_drive_mode: 2
+
+arm_1_joint_velocity_controller:
+  type: velocity_controllers/JointVelocityController


have you tested this on hardware?
does the ur_modern_driver support velocity interfaces? if yes, then please use ur_modern_driver in the ur.launch....with proper configuration....so far, this seems to be simulation only

I tested it at it works. I will keep the both options for the drivers for the ones that don't have the new driver... The configuration files are inside the driver package. I don't think it is a good idea to change that...

What did you test? On hardware?
In case it works with the (released) ur_modern_driver and in simulation, it's ok to use the ur_modern_driver as the default option in ur.launch...as long as the dependency is defined...

I will keep like this for now. If you guys want in the future to make it default feel free.

fmessmer

few more...

fmessmer · 2017-03-13T07:44:08Z

cob_moveit_config/raw3-1/config/controllers.yaml

    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - torso_pan_joint
      - torso_tilt_joint
+  - name: multi_dof_joint_trajectory_action
+    type: MultiDofFollowJointTrajectory


this won't work for others...that do not have this plugin

fmessmer · 2017-03-13T07:45:03Z

cob_moveit_config/raw3-1/config/base_controller.yaml

+    joints:
+      - virtual/x
+      - virtual/y
+      - virtual/theta


is this used by the MultiDofFollowJointTrajectory plugin? why not use base/x instead of virtual/x?

fmessmer · 2017-03-13T07:45:26Z

cob_moveit_config/raw3-1/config/fake_controllers.yaml

-  - name: fake_torso_controller
-    joints:
-      - torso_pan_joint
-      - torso_tilt_joint


keep the fake_torso_controller

fmessmer · 2017-03-13T07:45:53Z

cob_moveit_config/raw3-1/config/kinematics.yaml

-  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.05
-  kinematics_solver_attempts: 3


keep the torso kinematics plugin

fmessmer · 2017-03-13T07:46:18Z

cob_moveit_config/raw3-1/config/ompl_planning.yaml

@@ -26,14 +26,13 @@ planner_configs:
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  RRTkConfigDefault:
    type: geometric::RRT
-    range: 0.1  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()


if this PR is about adding STOMP...why do you change OMPL parameters?

fmessmer · 2017-03-13T07:47:02Z

cob_moveit_config/raw3-1/config/raw3-1.srdf

    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="world_joint" type="planar" parent_frame="odom_combined" child_link="base_footprint" />
+    <virtual_joint name="Base" type="planar" parent_frame="/odom_combined" child_link="base_footprint" />


why change the name?

it is more representative...

fmessmer

still:

restore indentation

and we have some dependency issues as the required packages are apparently not released into indigo

fmessmer · 2017-03-14T07:52:46Z

cob_bringup/components/legacy_schunk_powercube.launch

 	</group>
 </launch>
+


fully remove changes in this file...

fmessmer · 2017-03-14T07:53:09Z

cob_bringup/robots/cob4-5.xml

@@ -12,6 +12,7 @@
 	<arg name="env-script" default="$(find cob_bringup)/env.sh"/>
 	<arg name="sim" default="false"/>

+


fmessmer · 2017-03-14T07:54:22Z

cob_bringup/robots/cob4-5.xml

@@ -131,6 +132,7 @@
 			<arg name="component_name" value="arm_left"/>
 			<arg name="can_device" value="can3"/>
 			<arg name="sim" value="$(arg sim)"/>
+			<arg if="$(arg sim)" name="cartesian_control" value="$(arg cartesian_control)"/>


there is no $(arg cartesian_control...set value to true in case you like to use it...

also add it to arm_right

fmessmer · 2017-03-14T07:54:51Z

cob_bringup/robots/raw3-1.xml

@@ -68,6 +69,7 @@
 			<arg name="use_modern_driver" value="false"/>
 			<arg name="robot_ip" value="$(arg ur_ip)"/>
 			<arg name="sim" value="$(arg sim)"/>
+			<arg name="cartesian_control" value="$(arg cartesian_control)"/>


make consistent with the comments for cob4-5

fmessmer · 2017-03-14T07:55:15Z

cob_hardware_config/cob4-5/config/legacy_schunk_powercube.launch

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>


remove this file...it's in cob_bringup/components!

fmessmer · 2017-03-14T07:55:56Z

cob_hardware_config/raw3-1/config/arm_controller.yaml

+     - arm_wrist_3_joint
+  required_drive_mode: 1
+
+arm_1_joint_position_controller:


please respect the convention!

fmessmer · 2017-03-14T07:57:50Z

cob_hardware_config/raw3-1/config/arm_controller.yaml

+  required_drive_mode: 2
+
+arm_1_joint_velocity_controller:
+  type: velocity_controllers/JointVelocityController


What did you test? On hardware?
In case it works with the (released) ur_modern_driver and in simulation, it's ok to use the ur_modern_driver as the default option in ur.launch...as long as the dependency is defined...

fmessmer · 2017-03-14T08:00:15Z

cob_moveit_config/raw3-1/config/kinematics.yaml

  kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.005
+  kinematics_solver_timeout: 5


5 seconds for an IK solution?
You could use ur_kinematics/UR10KinematicsPlugin it's an analytic solver rather than the numerical KDL solver

This was the best configuration used... Trust me... I'm an engineer ;)

fmessmer · 2017-03-14T08:01:13Z

cob_moveit_config/raw3-1/config/ompl_planning.yaml

  RRTstarkConfigDefault:
    type: geometric::RRTstar
-    optimization_objective: MaximizeMinClearanceObjective


why did you change these values?
do the previous not result in a satisfying behavior?

fmessmer · 2017-03-14T08:01:58Z

cob_moveit_config/raw3-1/config/raw3-1.srdf

+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="arm">
+        <chain base_link="base_link" tip_link="gripper" />


is this group correct?
should it not be arm_base_link -> arm_ee_link?

fmessmer · 2017-04-05T09:32:46Z

cob_bringup/drivers/openni.launch

@@ -3,7 +3,7 @@

 	<arg name="robot" default="$(optenv ROBOT !!NO_ROBOT_SET!!)"/>
 	<arg name="name" default="cam3d"/>
-	<arg name="device_id" default="#1"/>
+	<arg name="device_id" default="2@0"/>


revert
call openni.launch from raw3-1.xml with proper launch argument instead

fmessmer · 2017-04-05T09:33:33Z

cob_bringup/drivers/ur.launch

+        <arg name="robot" value="$(arg robot)"/>
+        <arg name="component_name" value="$(arg component_name)"/>
+        <arg name="debug" value="false"/>
+    </include>


tabs vs. spaces

still tabs vs. spaces! please remove spaces and use tabs instead

fmessmer · 2017-04-05T09:38:04Z

cob_bringup/robots/cob4-5.xml

@@ -131,6 +131,7 @@
 			<arg name="component_name" value="arm_left"/>
 			<arg name="can_device" value="can3"/>
 			<arg name="sim" value="$(arg sim)"/>
+			<arg if="$(arg sim)" name="cartesian_control" value="true"/>


as long as we use the "old" cob_obstacle_distance we cannot use this on the real hardware because it leads to high load
however, I think if="$(arg sim)" is ok

but add it to to both arms!

fmessmer · 2017-04-05T09:38:38Z

cob_bringup/robots/raw3-1.xml

@@ -9,6 +9,7 @@
 	<arg name="ur_ip" default="localhost"/>
 	<arg name="env-script" default="$(find cob_bringup)/env.sh"/>
 	<arg name="sim" default="false"/>
+	<arg name="cartesian_control" default="true"/>


remove this line

fmessmer · 2017-04-05T09:39:37Z

cob_bringup/robots/raw3-1.xml

@@ -68,6 +69,7 @@
 			<arg name="use_modern_driver" value="false"/>
 			<arg name="robot_ip" value="$(arg ur_ip)"/>
 			<arg name="sim" value="$(arg sim)"/>
+			<arg if="$(arg sim)" name="cartesian_control" value="true"/>


ok, see https://github.com/ipa320/cob_robots/pull/631/files#r109871065

fmessmer · 2017-04-05T09:40:11Z

cob_hardware_config/raw3-1/config/arm_cartesian_controller.yaml

+
+# twist controller parameters
+twist_controller:
+  controller_interface: 0    #Velocity 0, Position 1, Trajectory 2, JointStates 3


use the new plugin-based interface configuration

preferably you meld this file against any other cartesian_controller.yaml for consistency...

fmessmer · 2017-04-05T09:41:58Z

cob_hardware_config/raw3-1/config/arm_controller.yaml

+     - arm_wrist_1_joint
+     - arm_wrist_2_joint
+     - arm_wrist_3_joint
+  required_drive_mode: 1


required_drive_mode is ros_canopen-specific...remove!

fmessmer · 2017-04-05T09:43:58Z

cob_moveit_config/raw3-1/config/kinematics.yaml

@@ -1,16 +1,11 @@
 arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin


try the analytical, UR-specific ur_kinematics/UR10KinematicsPlugin

Nor working for now:

Kinematics solver of type 'ur_kinematics/UR10KinematicsPlugin' could not be initialized for group 'arm'

We keep the previous solver.

the ur_kinematics/UR10KinematicsPlugin also takes a arm_prefix parameter...

fmessmer · 2017-04-05T09:45:04Z

cob_moveit_config/raw3-1/config/raw3-1.srdf

    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="world_joint" type="planar" parent_frame="odom_combined" child_link="base_footprint" />
+    <virtual_joint name="base" type="planar" parent_frame="/odom_combined" child_link="base_footprint" />


I guess you don't need the /

fmessmer · 2017-04-05T14:26:21Z

cob_bringup/drivers/ur.launch

+        <arg name="robot" value="$(arg robot)"/>
+        <arg name="component_name" value="$(arg component_name)"/>
+        <arg name="debug" value="false"/>
+    </include>


still tabs vs. spaces! please remove spaces and use tabs instead

fmessmer · 2017-04-05T14:26:49Z

cob_bringup/robots/cob4-5.xml

@@ -1,255 +1,257 @@
 <?xml version="1.0"?>
 <launch>


tabs vs. spaces

Now you want tabs??? I don't understand

it's always been tabs 😉

fmessmer · 2017-04-05T14:27:21Z

cob_hardware_config/raw3-1/config/arm_cartesian_controller.yaml

+  enforce_vel_limits: true
+  enforce_acc_limits: false
+  limits_tolerance: 5.0    #for position limits [deg]
+  #limits_acc: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]    #[rad/s2]


please update according to https://github.com/ipa320/cob_robots/blob/indigo_dev/cob_hardware_config/cob4-5/config/arm_left_cartesian_controller.yaml#L40-L49

fmessmer · 2017-04-05T14:40:59Z

cob_moveit_config/raw3-1/config/raw3-1.srdf

+    <group name="arm">
+        <chain base_link="arm_base_link" tip_link="arm_ee_link" />
+    </group>
+    <group name="robot">


when running roslaunch cob_moveit_bringup demo.launch robot:=raw3-1, I see

Group 'robot' does not have a parent link

Its normal because is creating a virtual joint to a link (odom_combined) that does not exist yet... after that is fine

fmessmer · 2017-04-05T14:41:43Z

cob_moveit_config/raw3-1/config/raw3-1.srdf

+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="arm">
+        <chain base_link="arm_base_link" tip_link="arm_ee_link" />


also, there is no end-effector defined for arm, thus, you do not get an interactive marker at the to move the GoalState in rviz!

I don't need that for the moment... THere was never a end-effector here xD

fmessmer · 2017-04-05T14:54:24Z

cob_moveit_config/raw3-1/config/kinematics.yaml

@@ -1,16 +1,11 @@
 arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin


the ur_kinematics/UR10KinematicsPlugin also takes a arm_prefix parameter...

bbrito · 2017-04-05T15:11:45Z

Also the official branch of ur arm does not use that plugin:

https://github.com/ros-industrial/universal_robot/blob/kinetic-devel/ur10_moveit_config/config/kinematics.yaml

changes made

floweisshardt added the waffle in progress label Mar 12, 2017

ipa-nhg added waffle review and removed waffle in progress labels Mar 12, 2017

floweisshardt removed the waffle review label Mar 12, 2017

floweisshardt previously requested changes Mar 12, 2017

View reviewed changes

fmessmer requested changes Mar 13, 2017

View reviewed changes

fmessmer requested changes Mar 14, 2017

View reviewed changes

fmessmer requested changes Apr 5, 2017

View reviewed changes

bbrito mentioned this pull request Apr 5, 2017

Stomp planner ipa320/cob_manipulation#104

Merged

bbrito and others added 12 commits April 5, 2017 17:34

merged stomp configuration with actual indigo_dev

164c06d

controllers for moveit namespace corrected

438c301

stomp configuration for raw3-1 created and tested

7e51db2

few corrections before pull request

d1f5d37

twist controller config for raw3-1

6651eb9

changes from pull request

a4e8bf0

new change from pull request

0004bd9

whole-body planning group: robot

2904626

stomp configuration for robot group

e964ed4

pull request changes

0edc3d0

stomp plannning yaml file correct group names

526dc05

twist controller config file updated to include input limits parameters

d296d14

bbrito force-pushed the stomp_planner branch from be7d9a6 to d296d14 Compare April 5, 2017 15:37

fmessmer approved these changes Apr 5, 2017

View reviewed changes

fmessmer force-pushed the stomp_planner branch from fb42501 to 327ef6e Compare April 5, 2017 16:36

finalizing PR

6d6ca8f

fmessmer force-pushed the stomp_planner branch from 327ef6e to 6d6ca8f Compare April 5, 2017 16:39

fmessmer merged commit a28deaa into ipa320:indigo_dev Apr 6, 2017

		@@ -12,6 +12,7 @@
		<arg name="env-script" default="$(find cob_bringup)/env.sh"/>
		<arg name="sim" default="false"/>

		@@ -1,16 +1,11 @@
		arm:
		kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin

Stomp planner #631

Stomp planner #631

Conversation

bbrito commented Mar 12, 2017 • edited by fmessmer Loading

floweisshardt left a comment

Choose a reason for hiding this comment

bbrito commented Mar 12, 2017

floweisshardt commented Mar 12, 2017

fmessmer left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmessmer left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmessmer left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bbrito commented Apr 5, 2017

bbrito commented Mar 12, 2017 •

edited by fmessmer

Loading