[Spot] add jsk spot robot

[sktometometo]

moved from #1325
based on #1263

Add jsk_spot_robot
This PR enables controlling spot with ROS.

[k-okada]

@sktometometo let's discuss once you finished your paper,
if this PR contains both stable packages and development packages, can we distinguish two part? Or can we consider all packages in this PR is stable and ready to merge?

[k-okada]

@708yamaguchi @mqcmd196 @tkmtnt7000 @sktometometo
please consider adding interface :body-pose

(:body-pose (r p y)
  (send self :body-inverse-kinematics ....)
  (setq ret (send .self .....))
  (setq ret-r (elt ret 0) ret-p (elt ret 1) ret-y (elt ret 2))
  (list ret-r ret-p ret-y))

So that we can visualize body-pose and send to real robot like

(send* *ri* :body-pose (send *spot* body-pose 0 0 -0.2))

[708yamaguchi]

@mqcmd196 @tkmtnt7000 @sktometometo

Is there a method or API to change height of spot body?

If so,
(:body-pose (x y z r p y)
or
(:body-pose (coords)
might be better.

[tkmtnt7000]

I think sending topic /spot/body_pose/position can change height of spot body.
http://www.clearpathrobotics.com/assets/guides/melodic/spot-ros/ros_usage.html#controling-the-body

As far as I know,
https://github.com/sktometometo/jsk_robot/blob/a23d26173fefbd5c99e32488b92aa570876898d5/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py#L259-L267
https://github.com/sktometometo/jsk_robot/blob/a23d26173fefbd5c99e32488b92aa570876898d5/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py#L178-L182
are now used to change height of spot body in elevator.

[708yamaguchi]

Thank you.

We also need to add spot-interface method to use /spot/body_pose/position

[mqcmd196]

@tkmtnt7000 @708yamaguchi I tried z with various values and it works fine.
However when I published like,

 rostopic pub /spot/bo_pose geometry_msgs/Pose "position:
  x: 0.0
  y: 0.0
  z: 0.5
orientation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0"

it doesn't mean 0.5m high from the ground, but 0.5m high from the spot normal height. I have to fix this issue to use from eus model.

[708yamaguchi]

it doesn't mean 0.5m high from the ground, but 0.5m high from the spot normal height. I have to fix this issue to use from eus model.

I think this is the same as current spot.l.

When we create *spot* instance, spot body coords is the same as eus origin coords.
For example, spot body coords (the red arrows) is the same as eus origin coords (rgb lines)
https://user-images.githubusercontent.com/19769486/123687288-b560b600-d88b-11eb-8183-fb36c108fe99.png

Therefore, in :body-inverse-kinematics, we can use coords argument as relative coords from spot normal posture.

[708yamaguchi]

One problem with legged robot is that it is difficult to define ground coords.
Like,
Should ground coords be center of gravity?? center of legs?
if 3 of 4 legs are on the ground, how should we define the ground coords?

I think this is why /spot/body_pose uses relative pose from spot normal posture.

[mqcmd196]

I think this is why /spot/body_pose uses relative pose from spot normal posture.

I see, this not happens in cart type robot.

And it seems *spot* returns relative coords from spot normal posture. I misunderstood.