Use interactive markers to control iiwa14 joints #3

sloretz · 2020-09-01T16:20:38Z

Add Demo using interactive markers to control iiwa14 in drake.

This uses pydrake systems framework, the manipulation station, and ROS 2 Rolling with ros/sdformat_urdf#1 to control the robot in joint space with interactive markers on each joint.

@IanTheEngineer FYI - see interactive_demo.py

Roughly to run this:

Build a ROS 2 workspace with Initial urdf parser plugin that parses sdformat ros/sdformat_urdf#1 and source it in a terminal
Download the latest binary install of drake
Use @EricCousineau-TRI's trick of python3 -m venv --system-site-packages path/to/extracted/drake and activate the venv to make Drake usable from that terminal
In another terminal activate the venv and run drake-visualizer
In another terminal activate the venv and run rviz2 -d path/to/this/repo/view.rviz
Run python3 ./interactive_demo.py

sloretz · 2020-10-20T16:31:58Z

@EricCousineau-TRI @azeey mind having a look?

EricCousineau-TRI · 2020-11-03T16:42:53Z

Taking a look now!

EricCousineau-TRI

Will run it on my machine

iiwa14_no_collision.sdf.in

interactive_demo.py

iiwa14_no_collision.sdf.in

drake_ros/drake_ros/systems/__init__.py

interactive_demo.py

azeey

LGTM! I was able to run this locally (in docker) after fixing a couple of issues I found (mentioned in comments). Given that this is a prototype, feel free to ignore some of my comments. As a general comment, I wonder how hard it would be to just publish joint states and use the Robot state publisher to do the TF computation instead of having the TFPublisher system here. This would further exercise the sdformat_urdf package

drake_ros/drake_ros/systems/movable_joints.py

drake_ros/drake_ros/systems/movable_point.py

interactive_demo.py

sloretz · 2020-11-16T23:57:29Z

@EricCousineau-TRI @azeey Feedback has been addressed. Mind taking another look?

EricCousineau-TRI

Can you use .gitattributes to suppress diffs for .obj files here?
https://github.com/RobotLocomotion/drake/blob/4731f31ad39753dfde25611456e55f98d027d469/.gitattributes

interactive_demo.py

azeey

LGTM. Mind adding the AMENT_PREFIX_PATH stuff to the README as well?

sloretz · 2020-11-30T19:48:52Z

Can you use .gitattributes to suppress diffs for .obj files here?

Suppressed in f07b054

Signed-off-by: Shane Loretz <[email protected]>

Quickly gets stuck in singularity Maybe add routine to re-home robot in joint space when Diff_ik fails? Signed-off-by: Shane Loretz<[email protected]> Signed-off-by: Shane Loretz <[email protected]>

Signed-off-by: Shane Loretz <[email protected]>

Co-authored-by: Addisu Z. Taddese <[email protected]>

Signed-off-by: Shane Loretz <[email protected]>

Uses stub package.xml files and an extra ament index to convince both ROS and Drake that the SDFormat XML files and meshes live in a ROS package. This allows using `package://` URLs instead of the temporary file made from populating a template with absolute `file://` URIs. To launch: AMENT_PREFIX_PATH="$AMENT_PREFIX_PATH:$(pwd)" python3 ros2_demo.py AMENT_PREFIX_PATH="$AMENT_PREFIX_PATH:$(pwd)" rviz2 -d view.rviz Signed-off-by: Shane Loretz <[email protected]> Signed-off-by: Shane Loretz <[email protected]>

Signed-off-by: Shane Loretz <[email protected]>

Gets rid of templated sdformat file To launch: AMENT_PREFIX_PATH="$AMENT_PREFIX_PATH:$(pwd)" python3 interactive_demo.py AMENT_PREFIX_PATH="$AMENT_PREFIX_PATH:$(pwd)" rviz2 -d interactive_demo.rviz Signed-off-by: Shane Loretz <[email protected]> Signed-off-by: Shane Loretz <[email protected]>

Signed-off-by: Shane Loretz <[email protected]>

sloretz · 2020-11-30T20:07:18Z

LGTM. Mind adding the AMENT_PREFIX_PATH stuff to the README as well?

Rebased and updated the README in 4bd74d1

sloretz · 2020-11-30T20:08:48Z

It looks like all the comments have been addressed. Since this is a prototype repo, I'm inclined to merge after giving a day or so for additional reviews.

sloretz · 2020-12-03T00:20:25Z

Seemed like there was no opposition to merging in yesterday's meeting, so merging 🎉

EricCousineau-TRI · 2020-12-03T18:34:16Z

Sweet, thank you!!!

sloretz self-assigned this Sep 1, 2020

sloretz marked this pull request as draft September 1, 2020 16:20

sloretz changed the title ~~Whatever is here - segfaults~~ Whatever is here - controls robot - ish Sep 2, 2020

sloretz changed the title ~~Whatever is here - controls robot - ish~~ Use interactive markers to control iiwa14 joints Oct 19, 2020

sloretz marked this pull request as ready for review October 19, 2020 21:06

EricCousineau-TRI reviewed Nov 3, 2020

View reviewed changes

azeey reviewed Nov 10, 2020

View reviewed changes

drake_ros/drake_ros/systems/__init__.py Outdated Show resolved Hide resolved

interactive_demo.py Show resolved Hide resolved

azeey reviewed Nov 10, 2020

View reviewed changes

interactive_demo.py Show resolved Hide resolved

interactive_demo.py Show resolved Hide resolved

azeey approved these changes Nov 10, 2020

View reviewed changes

drake_ros/drake_ros/systems/movable_joints.py Outdated Show resolved Hide resolved

drake_ros/drake_ros/systems/movable_point.py Outdated Show resolved Hide resolved

interactive_demo.py Outdated Show resolved Hide resolved

EricCousineau-TRI reviewed Nov 19, 2020

View reviewed changes

interactive_demo.py Show resolved Hide resolved

EricCousineau-TRI mentioned this pull request Nov 21, 2020

pydrake bazel: How to get a stacktrace in debug mode? RobotLocomotion/drake#14381

Closed

azeey approved these changes Nov 23, 2020

View reviewed changes

sloretz added 13 commits November 30, 2020 11:51

Whatever is here - segfaults

0f92a4a

Signed-off-by: Shane Loretz <[email protected]>

Marker controls robot-ish

bf1b654

Quickly gets stuck in singularity Maybe add routine to re-home robot in joint space when Diff_ik fails? Signed-off-by: Shane Loretz<[email protected]> Signed-off-by: Shane Loretz <[email protected]>

Remove print statements

293b432

Signed-off-by: Shane Loretz <[email protected]>

Almost does what I want, diffik fails pretty quickly

a459c22

Signed-off-by: Shane Loretz <[email protected]>

At least it runs - does nothing useful but outputs to RViz again

dd59fa9

Signed-off-by: Shane Loretz <[email protected]>

Add changes for joint space control - not sure what works

5e3ebbb

Signed-off-by: Shane Loretz <[email protected]>

Better view

5c021ed

Signed-off-by: Shane Loretz <[email protected]>

Almost does the right rotation of marker

b6ea7a0

Signed-off-by: Shane Loretz <[email protected]>

Fix bug where axis was affected by translation to F

6f0572c

Signed-off-by: Shane Loretz <[email protected]>

Faster update rate!

9af2a09

Signed-off-by: Shane Loretz <[email protected]>

Increment joint angle (still two joints go opposite of what I'd expect)

6d2e79e

Signed-off-by: Shane Loretz <[email protected]>

Get rid of computed sdf

f582a68

Signed-off-by: Shane Loretz <[email protected]>

Marker in child frame

3e93951

Signed-off-by: Shane Loretz <[email protected]>

sloretz and others added 20 commits November 30, 2020 11:51

Behaves as expected

ce9221f

Signed-off-by: Shane Loretz <[email protected]>

Cleanup unused code

af8bd1a

Signed-off-by: Shane Loretz <[email protected]>

Delete unused file

c390f4e

Signed-off-by: Shane Loretz <[email protected]>

Copy view.rviz -> interactive_demo.rviz

bbf5a05

Signed-off-by: Shane Loretz <[email protected]>

Reset view.rviz for ros2_demo.py

f4c8b70

Signed-off-by: Shane Loretz <[email protected]>

Removed unused imports

3177e88

Signed-off-by: Shane Loretz <[email protected]>

Wrap in main()

277d64f

Signed-off-by: Shane Loretz <[email protected]>

Use drake's isort settings

0423c90

Signed-off-by: Shane Loretz <[email protected]>

Moveable -> Movable

935e414

Signed-off-by: Shane Loretz <[email protected]>

moveable -> movable

becd67d

Signed-off-by: Shane Loretz <[email protected]>

Workaround manipulation station python binding bug

c17d305

Signed-off-by: Shane Loretz <[email protected]>

Update drake_ros/drake_ros/systems/movable_joints.py

870efc3

Co-authored-by: Addisu Z. Taddese <[email protected]>

Import ComputeBasisFromAxis

f34bee3

Signed-off-by: Shane Loretz <[email protected]>

Remove unused PadVector system

b26f908

Signed-off-by: Shane Loretz <[email protected]>

Remove unused MovablePoint system

265b5d6

Signed-off-by: Shane Loretz <[email protected]>

os.path.join() instead of +

a092f27

Signed-off-by: Shane Loretz <[email protected]>

Suppress diffs on obj files

2c3224a

Signed-off-by: Shane Loretz <[email protected]>

Update README for AMENT_PREFIX_PATH and interactive demo

4bd74d1

Signed-off-by: Shane Loretz <[email protected]>

sloretz force-pushed the interactive_demo branch from f07b054 to 4bd74d1 Compare November 30, 2020 20:05

sloretz merged commit 3bb1c5c into master Dec 3, 2020

sloretz deleted the interactive_demo branch December 3, 2020 00:20

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Use interactive markers to control iiwa14 joints #3

Use interactive markers to control iiwa14 joints #3

sloretz commented Sep 1, 2020 •

edited

Loading

sloretz commented Oct 20, 2020

EricCousineau-TRI commented Nov 3, 2020

EricCousineau-TRI left a comment

azeey left a comment

sloretz commented Nov 16, 2020

EricCousineau-TRI left a comment

azeey left a comment

sloretz commented Nov 30, 2020

sloretz commented Nov 30, 2020

sloretz commented Nov 30, 2020

sloretz commented Dec 3, 2020

EricCousineau-TRI commented Dec 3, 2020

Use interactive markers to control iiwa14 joints #3

Use interactive markers to control iiwa14 joints #3

Conversation

sloretz commented Sep 1, 2020 • edited Loading

sloretz commented Oct 20, 2020

EricCousineau-TRI commented Nov 3, 2020

EricCousineau-TRI left a comment

Choose a reason for hiding this comment

azeey left a comment

Choose a reason for hiding this comment

sloretz commented Nov 16, 2020

EricCousineau-TRI left a comment

Choose a reason for hiding this comment

azeey left a comment

Choose a reason for hiding this comment

sloretz commented Nov 30, 2020

sloretz commented Nov 30, 2020

sloretz commented Nov 30, 2020

sloretz commented Dec 3, 2020

EricCousineau-TRI commented Dec 3, 2020

sloretz commented Sep 1, 2020 •

edited

Loading