search

uwgraphics / relaxed_ik

RelaxedIK Solver
MIT License
186 stars 50 forks source link

Kinova Mico URDF and collision obstacles not aligned #2

Closed sjhansen3 closed 5 years ago

sjhansen3 commented 6 years ago

I am running the start_here.py demo on Ubuntu 16.04 ROS kinetic with an m1n6s300 Kinova mico arm.

Attached is my URDF, start_here.py file and config.yaml. All seems fine until step 5b. Rviz seems to show the joint angles are off from the collision objects. Seen below in the screen shot.

screenshot from 2018-08-24 18-12-09

kinova_ik_error.tar.gz

I am sure its something to do with my configuration/URDF etc, but I cant seem to figure out what. Any hints are welcome. :)

sjhansen3 commented 6 years ago

I thought I would also add that there are a few warnings at the beginning of the launch file:

sjhansen3@sjhansen3:~/peanut_ws/src/relaxed_ik$ roslaunch relaxed_ik collision_viewer.launch ... logging to /home/sjhansen3/.ros/log/710753aa-a806-11e8-a5ac-40a3ccb4dbba/roslaunch-sjhansen3-31035.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://sjhansen3:39231/

SUMMARY

PARAMETERS

NODES / collision_viewer (relaxed_ik/collision_viewer.py)

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

setting /run_id to 710753aa-a806-11e8-a5ac-40a3ccb4dbba process[rosout-1]: started with pid [31058] started core service [/rosout] process[collision_viewer-2]: started with pid [31066] ... logging to /home/sjhansen3/.ros/log/710753aa-a806-11e8-a5ac-40a3ccb4dbba/roslaunch-sjhansen3-31066.log started roslaunch server http://sjhansen3:39619/

SUMMARY

PARAMETERS

NODES / robot_state_publisher (robot_state_publisher/robot_state_publisher) rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311 process[rviz-1]: started with pid [31112] process[robot_state_publisher-2]: started with pid [31113] Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Unknown tag: hardwareInterface Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Scalar element defined multiple times: visual Unknown tag: hardwareInterface Unknown tag: hardwareInterface Unknown tag: hardwareInterface

djrakita commented 6 years ago

Hi! It looks like the issue here was the solver not supporting the "continuous" joint type, which is present on the mico robot's base joint. I just added an update that adds support for that joint type, so all should be good to go.

Thanks for bringing that to my attention, just let me know if you have any other issues or questions!

djrakita commented 6 years ago

Also, as far as the "Unknown tag" warnings, those are just outputted by the urdf parser, basically saying "I don't understand how to interpret these elements". I'm sure there's some way to prevent those warnings from spamming the console on startup, I'll look into it :) In the meantime, just know that they're totally fine and don't indicate any issue with the solver or anything

djrakita commented 6 years ago

One other thing I thought I'd mention: after running the neural network learning in preprocessing, if it seems like the mico is getting too close to self-collisions, it may be worth trying to give the collision sample states a little bit more breathing room. Hopefully it works given the ones you provided, but just thought I'd mention that tip here in case you don't get desired results right off the bat!

sjhansen3 commented 6 years ago

Hi - Thanks for the reply. Your change did indeed fix the issue where the cylinders would not align. But I cant seem to add any additional things like spheres or elpsoids to cover the fingers. Could you provide an example .yaml file which does this for the mico?

Here is my current yaml coinfig which does not show a sphere anywhere - perhaps I dont understand what the coordinate frame number means or I am misusing the yaml file in some way :)

robot_link_radius: 0.05
sample_states: [
  [-1.01, 4.40, 1.15, -1.36, 0.30, -3.14],
  [-2.04, 2.04, 5.67, -2.54, -0.03, 0.19],
  [-0.44, 2.98, 4.60, 1.57, 2.78, -2.15],
  [-1.57, 2.55, 0.97, -1.24, -2.38, 1.32],
  [1.58, 4.726, 5.419, 1.624, 0.794, 0.0],
  [1.279, 1.797, 5.157, 0.391, -2.383, -1.568],
  [1.7178, 2.727, 5.443, 0.131, -1.624, -0.248],
  [-2.80, 1.93, 0.94, -2.76, -2.11, -2.46]
]
boxes:
spheres:
  - name: sphere1
    parameters: .2
    coordinate_frame: 4
    rotation: [0,0,0]
    translation: [.115,0.025,-0.053]
ellipsoids:
capsules:
cylinders:
sjhansen3 commented 6 years ago

I have another question regarding the training loss and comparisons...What is good and what is bad? Is there any benchmarking? What do the numbers indicate in the output:

299999 of 300000 samples: 4.530384449369481e-16
Iteration 1, loss = 74.70444674
Iteration 2, loss = 42.53073484
Iteration 3, loss = 37.95542712
Iteration 4, loss = 35.33101741
Iteration 5, loss = 33.83802667
Iteration 6, loss = 26.91409363
Iteration 7, loss = 20.24643228
Iteration 8, loss = 17.44807868
Iteration 9, loss = 16.68838661
Iteration 10, loss = 15.42927831
Iteration 11, loss = 14.62737196
Iteration 12, loss = 14.42898119
Iteration 13, loss = 13.68895267
Iteration 14, loss = 13.59297048
Iteration 15, loss = 12.86419079
Iteration 16, loss = 12.51971788
Iteration 17, loss = 12.36943210
Iteration 18, loss = 11.75138647
Iteration 19, loss = 11.70389053
Iteration 20, loss = 11.34332752
Iteration 21, loss = 11.23371725
Iteration 22, loss = 10.95542683
Iteration 23, loss = 10.45964917
Iteration 24, loss = 10.33334163
Iteration 25, loss = 10.26342128
Iteration 26, loss = 10.19577208
Iteration 27, loss = 10.09494121
Iteration 28, loss = 9.96221595
Iteration 29, loss = 9.70750994
Iteration 30, loss = 9.54976139
Iteration 31, loss = 9.50294981
Iteration 32, loss = 9.10978847
Iteration 33, loss = 9.35808042
Iteration 34, loss = 8.95434632
Iteration 35, loss = 8.77055819
Iteration 36, loss = 8.84173644
Iteration 37, loss = 8.60636169
Iteration 38, loss = 8.63388088
Iteration 39, loss = 8.34294977
Iteration 40, loss = 8.67186201
Iteration 41, loss = 8.60821738
Iteration 42, loss = 8.18448278
Iteration 43, loss = 8.14550766
Iteration 44, loss = 8.26721013
Iteration 45, loss = 8.15114335
Iteration 46, loss = 7.79530969
Iteration 47, loss = 7.65908801
Iteration 48, loss = 8.00871206
Iteration 49, loss = 7.54861871
Iteration 50, loss = 7.59678917
Iteration 51, loss = 7.75792308
Iteration 52, loss = 7.40720765
Iteration 53, loss = 7.43127132
Iteration 54, loss = 7.45332406
Iteration 55, loss = 7.27461027
Iteration 56, loss = 7.35910256
Iteration 57, loss = 7.48152885
Iteration 58, loss = 7.16769303
Iteration 59, loss = 7.12638225
Iteration 60, loss = 7.20770080
Iteration 61, loss = 7.05406154
Iteration 62, loss = 7.09928088
Iteration 63, loss = 7.08537021
Iteration 64, loss = 7.27423848
Training loss did not improve more than tol=0.000100 for two consecutive epochs. Stopping.
output comparisons...
[-0.00122671]
1.877061473904583e-25

[-0.00296925]
1.7931633938614476e-123

[0.00448173]
1.27998910270689e-18

[0.00202198]
2.5648579647062224e-140

[-0.0009235]
3.494956107415663e-16

[0.00318]
6.305377853263185e-219

[-0.00510468]
1.27658765539556e-125

[0.53183536]
3.155880560053222e-18

[0.00459502]
2.738867200181041e-161

[0.00326351]
1.5987970860440201e-52

[-0.00075004]
8.494608380518297e-17

[-0.00369043]
0.0

[73.26713916]
88.91533233800533

[-0.00570697]
1.4657488180102925e-65

[-0.0080134]
1.490413279807016e-206

[0.00139505]
2.8660249508335975e-45

[-0.00048427]
1.2807111145485316e-15

[0.00311373]
1.9935922959872114e-17

[-0.00235252]
1.7071226703054265e-19

[-0.00068052]
8.661845386834344e-22

[0.00150121]
0.0

[0.00194517]
8.22520748197639e-49

[-0.00202809]
4.661906886419465e-16

[0.00064526]
1.6463091437217881e-22

[-0.00567197]
7.011783937456342e-115

[-0.00585333]
6.022346267122211e-184

[0.00164538]
1.377534861651858e-15

[11.01535623]
5.432671969836504

[0.00572129]
7.217459255485342e-25

[0.00442013]
4.2657855981473553e-187

[0.00112114]
0.0

[-0.00012661]
7.486941658010743e-17

[-0.00603381]
1.3743953297973094e-257

[-0.00616357]
1.5234228615216657e-164

[0.00195262]
2.9226242926186525e-208

[-0.00289429]
2.328029002327747e-144

[0.00440117]
5.1079092373495803e-39

[-0.00359884]
2.3503471776278388e-46

[0.00132803]
6.725722950888792e-118

[0.00174689]
6.4469867165546e-256

[-0.00498564]
1.4210910506533021e-167

[-0.00502676]
4.621811477723388e-300

[0.00662039]
4.032604514640851e-118

[0.03071878]
2.1014432345966214e-05

[-0.00550079]
4.3320016067546945e-170

[0.00246783]
1.165154221193761e-10

[-0.00238625]
4.0453275514883773e-16

[-0.00733563]
1.8504015218048754e-191

[-0.00363582]
1.1384114391943872e-13

[0.00460658]
4.659561550381207e-13

[0.00160791]
4.274624177072878e-229

[-0.00141854]
3.5330197711558974e-118

[-0.00090526]
8.886995897328786e-11

[-0.00229655]
1.6949703140221984e-240

[0.00261797]
8.422497161462183e-16

[0.00254934]
0.0

[0.00240996]
2.3570978288328704e-15

[0.0061866]
8.833946348527403e-33

[-0.00106169]
1.1125195995959284e-16

[0.00446627]
4.0811839211469314e-14

[0.00152519]
0.0

[0.00144728]
8.694824515643237e-139

[0.00359392]
5.768166922301822e-42

[-0.00610019]
1.628315537522116e-173

[0.00230193]
4.190408384583105e-16

[-0.00134575]
1.28751677756612e-15

[0.00456253]
5.267394005079212e-13

[0.00402399]
1.1294905198500256e-97

[-0.00276218]
2.2866289428630604e-12

[90.17149554]
100.0

[0.00495907]
2.3245302799926925e-21

[0.00069494]
2.361452550676274e-254

[0.0053577]
1.370427944369671e-15

[0.00433485]
8.388278375421368e-133

[0.00471849]
5.469869737562516e-51

[0.00629472]
5.9876980204935114e-31

[-0.00161349]
1.4525955578863352e-202

[0.00038117]
1.4475015177068552e-15

[0.00686861]
7.415458353798592e-16

[0.00714858]
1.9981758680808007e-11

[0.00146513]
0.0

[-0.00415215]
4.932195157455632e-16

[-0.00245786]
1.0671918043069351e-13

[0.00553552]
4.8191790120510285e-23

[88.26115139]
88.86658631405906

[-0.0057937]
6.344540637328291e-23

[-0.00035158]
1.527297509963562e-43

[76.88903999]
63.68758456761366

[-0.00216109]
2.118200006448049e-12

[0.00712933]
8.055636257845058e-29

[-0.00015878]
3.835450468633872e-16

[0.00273533]
2.3146911168004035e-128

[-0.00505134]
2.9571765232099934e-156

[0.00466066]
1.2726023808399162e-39

[-0.00374583]
1.6623579363018037e-28

[0.0063217]
1.1958737956550572e-66

[0.00068224]
9.335940310294754e-50

[-0.00465132]
8.08066397186684e-157

[0.00342464]
5.240347271436576e-21

[0.00096839]
1.9418526791639167e-12

These dont look bad, or great... I am unsure if matching 90-100 is good or bad. Is there anything to benchmark against? How did other models you trained perform? Can you explain a bit more about the training process? Which states are used for training and how are the sample states utilized for the training process?

djrakita commented 6 years ago

Hey, I've been able to reproduce the error where the sphere doesn't show up in rviz, even when specified in the yaml. For some reason, if I copy and paste the same sphere below that first sphere, it starts showing up in rviz. I haven't been able to figure out why it skips drawing the first sphere in the list (I'm still unsure if it's on my side or the rviz side). I'll try to find a more permanent fix for this, but just know that a short term fix is to just add the sphere to the yaml file again and it shows up in rviz