um-arm-lab / arm_robots Goto Github PK

Right now val is on 18.04 and clearpath does not plan to support 20.04 anytime soon (they will eventually, just not in the next few months). This means there's no good way to run python3 ros code. I'm making this issue just so people know, and so we can discuss workarounds if someone needs to run python3 code on val.

The main issue this causes is that none of the arm_robots python code can run on val, you have to run it on a separate computer. So far this isn't a huge problem, but the latency and reliance on a separate computer is not great

Changing the EE link you're planning to yields a multiple constraint issue. E.g.,

med.plan_to_position(med.arm_group, "grasp_frame", [0.5, 0.1, 0.3])
med.plan_to_pose(med.arm_group, "med_kuka_link_ee", [0.5, -0.1, 0.35, 0.0, np.pi, 0.0])

fails to find a plan and yields the following warning:

More than one constraint is set. If your move_group does not have multiple end effectors/arms, this is unusual. Are you using a move_group_interface and forgetting to call clearPoseTargets() or equivalent?

Currently the trajectory follower will continue running after the victor object is destroyed (e.g. if you ctrl-c).
Better behavior would be that ctrl-c stops victor.

This can be accomplished through the stop callback

smile@smile-X3-S:~/ros_catkin_ws$ roslaunch arm_robots victor.launch sim:=true
... logging to /home/smile/.ros/log/c5d7f5cc-4af7-11ed-875a-3c58c26f458c/roslaunch-smile-X3-S-2487.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

Resource not found: victor_moveit_config
ROS path [0]=/opt/ros/melodic/share/ros
ROS path [1]=/home/smile/ros_catkin_ws/src
ROS path [2]=/home/smile/original_code_compile/src
ROS path [3]=/home/smile/app_install/catkin_ws/src
ROS path [4]=/opt/ros/melodic/share
The traceback for the exception was written to the log file

Starting victor.launch produces this error:
[ERROR][/victor/move_group]: No sensor plugin specified for octomap updater 0; ignoring

It would be nice to not show this, as it will be confusing to new users

We need a quick tutorial demonstrating how to connect to the robot in simulation and real victor

Currently the goal_tolerance in the trajectory follower for victor is set to 0.2 rad.
This is fine for joint position mode and many but not all configurations in impedance mode with stiff parameters

One option for impedance mode is let the kuka controller say when it has reached the destination after setting the last waypoint (and therefore we wont expect the kuka controller to improve much further). This would involve adding a new field passed from java.

Another option is make the goal tolerance settable and have utilities for setting reasonable goal tolerances

Joint names, velocities, accels are listed in kuka_iiwa_interface as vell as victor_config.py.

We should figure out how to pull these in to arm_robots without explicitly duplicating them in victor_config

When running victor.follow_jacobian_to_position(...) twice in a row, the following error message appears repeatedly and victor fails to execute the second jacobian follow.

Repo steps:
Terminal 1: roslaunch arm_robots rviz_victor.launch launch_fake_dual_arm_bridge:=true
Terminal 2: rviz -d [arm_robots]/rviz/victor.rviz pycharm: victor_basic_motion.pyorjacobian_follwer_bug.py`. Bug appears after "Follow jacobian to pose 2?"

[WARN][/victor_basic_motion]: Robot is late in reaching goal. Error is:
victor_right_arm_joint_1: -0.05
victor_right_arm_joint_2: -0.15
victor_right_arm_joint_3: -0.05
victor_right_arm_joint_4: -0.10
victor_right_arm_joint_5: 0.05
victor_right_arm_joint_6: 0.10
victor_right_arm_joint_7: 0.01

Thoughts:
victor.block = True, however the blocking does not seems to be working. The bug disappears if you add a long enough rospy.sleep(5) between the follow_jacobian calls.
Perhaps this issue only appears when using fake victor, as fake victor immediately reaches any goal position. Untested.

Some functions take a PlanningScene and a RobotState. That's confusing because PlanningScene includes a robot state, which currently is ignored.

ex: check_collision(moveit_msgs::PlanningScene const &scene_msg, moveit_msgs::RobotState const &state_msg)

The IK exposed in the jacobian follower relies on setIkSolverFrame, which will be made private in the next release of moveit.

See: UM-ARM-Lab/kuka_iiwa_interface#130

One issue is lab members might look for manual_motion in arm_robots, since arm_robots has most of the other robot motion functions. Maybe leave a manual_motion launch script that just points to the KHI/Med version?

In order to execute with a stop condition, you currently need to disable execution, plan, then re-enable execution, and pass the plan + stop condition to the trajectory execution function. We should add an optional stop_condition argument to all planning functions (e.g., plan_to_pose, plan_to_position, plan_to_position_cartesian, etc.) that gets sent to the trajectory execution function if execution is enabled.

This is probably the single biggest naming problem in our entire code base. All the code HDT wrote calls it "hdt_mihigan" but we have used val in a lot of places. Accepting that it's called hdt_michigan in code is the far easier option, and so we could go forward with this convention. We can still cal it val casually in that case. Or we can use val in code going forward, and eventually drop or rename places it was called hdt_michigan. I'm personally in favor of keeping hdt_michigan but I could be convinced otherwise.

This demo script currently has multiple issues
1. Tries to look up "home" position, which does not exist,
2. Kuka controller errors when running plan_to_position_cartesian (line 69)

In simulation Victor will collision check the path. On the real robot collision checking does not happen (or always reports no collision)

In a specific failure on startup victor will move to [0,0,0,0,0,0,0] instead of the planned position. This could lead to crashes.

If the joint_state_publisher on realtime crashes, the arm_robots move_group node will fail to "fetch the current robot state". The robot then moves to [0,0,0,0,0,0,0].

I am not sure why, but suspect the robot thinks it is at [0,0,0,0,0,0,0], and thus follow_joint_trajectory thinks it is commanding a small move.

Output from victor.launch after joint_state_publisher has died.

You can start planning now!

[ INFO][/victor/move_group]: Didn't received robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ WARN][/victor/move_group]: Failed to fetch current robot state.
[ INFO][/victor/move_group]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO][/victor/move_group]: Planner configuration 'right_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO][/victor/move_group]: right_arm/right_arm: Starting planning with 1 states already in datastructure
[ INFO][/victor/move_group]: right_arm/right_arm: Created 5 states (2 start + 3 goal)
[ INFO][/victor/move_group]: Solution found in 0.020732 seconds
[ INFO][/victor/move_group]: SimpleSetup: Path simplification took 0.049736 seconds and changed from 4 to 2 states

The recommended fix is to detect this error, throw an exception, and shut down the node(s)

See:

If arm_robots sends a velocity command greater than the Kuka Controller's allowed command the Kuka controller will cause an "Internal Interpolation error`

Currently Moveit an produce plans arbitrarily close to collision, or possible even in collision. During many moves there is no need to cut the margin of safety so close. This almost certainly will cause physical robot collisions eventually because:

• Victor/Val have significant error between URDF/simulation and physical robot
• The gripper collision does not exactly match the true gripper
• We will have errors in modeled objects
• Moveit can produce plans in collision due to discretization

One approach is to create a separate padded URDF, and plan using the {padded/tight} URDF depending on the situation

A better approach would be to directly have this feature in moveit. We are not the only ones:
moveit/moveit#683

Display goal not working if plan_to_joint_config is given not a dictionary.

launching victor.launch, killing, then relaunching can cause realtime's joint_state_publisher to crash because launching gazebo can cause a jump backwards in time.

Traceback (most recent call last):
  File "/home/armlab/catkin_ws/src/kuka_iiwa_interface/victor_hardware_interface/scripts/joint_state_publisher.py", line 178, in <module>
    pub.run(rate)
  File "/home/armlab/catkin_ws/src/kuka_iiwa_interface/victor_hardware_interface/scripts/joint_state_publisher.py", line 114, in run
    rate.sleep()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 103, in sleep
    sleep(self._remaining(curr_time))
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 163, in sleep
    raise rospy.exceptions.ROSTimeMovedBackwardsException(time_jump)
rospy.exceptions.ROSTimeMovedBackwardsException: ROS time moved backwards
[victor/joint_state_publisher-1] process has died [pid 59304, exit code 1, cmd /home/armlab/catkin_ws/src/kuka_iiwa_interface/victor_hardware_interface/scripts/joint_state_publisher.py __name:=joint_state_publisher __log:=/home/armlab/.ros/log/bc0ab0b6-4b5d-11eb-a4fe-3500f8258116/victor-joint_state_publisher-1.log].
log file: /home/armlab/.ros/log/bc0ab0b6-4b5d-11eb-a4fe-3500f8258116/victor-joint_state_publisher-1*.log

Related to #7