Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix :move-to for non-holonomic robot #405

Closed
wants to merge 2 commits into from
Closed

fix :move-to for non-holonomic robot #405

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e5c8f09
fix :move-to for non-holonomic robot
knorth55 Oct 23, 2019
12a5a40
update :move-to for non-holonomic robot
knorth55 Oct 24, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 27 additions & 1 deletion pr2eus/robot-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1241,20 +1241,23 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
base-frame-id
odom-topic move-base-trajectory-joint-names
go-pos-unsafe-goal-msg
current-goal-coords)) ;; for simulation-callback
current-goal-coords ;; for simulation-callback
holonomic))
(defmethod robot-move-base-interface
(:init
(&rest args &key
(move-base-action-name "move_base") ((:base-frame-id base-frame-id-name) "base_footprint")
(base-controller-action-name "/base_controller/follow_joint_trajectory")
(base-controller-joint-names (list "base_link_x" "base_link_y" "base_link_pan"))
(holonomic t)
((:odom-topic odom-topic-name) "/base_odometry/odom") &allow-other-keys)
(prog1 (send-super* :init args)
(setq base-frame-id base-frame-id-name)
(setq odom-topic odom-topic-name)
(setq move-base-action (instance ros::simple-action-client :init
move-base-action-name move_base_msgs::MoveBaseAction
:groupname groupname))
(setq holonomic holonomic)
(when base-controller-action-name
(setq move-base-trajectory-action
(instance ros::simple-action-client :init
Expand Down Expand Up @@ -1424,6 +1427,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
;;
(dotimes (i 2)
(if (< (setq diff-len (norm (subseq (send diff :worldpos) 0 2))) 200) ;; move_base thre = 200mm
(if holonomic
Copy link
Member

@k-okada k-okada Oct 25, 2019 •
edited by pazeshun
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can we use :go-pos-unsafe instead of :go-velocity ? if this works, then it chance to remove this line and use go-pos-unsafe for both holonoic and non-holonomic robot.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we can use :go-pos-unsafe for pr2 robot.
but for fetch, we need to tune :rotation-gain in :go-pos-unsafe
that is because of friction between wheel and the ground.
for rotating 45 degree, it is better to set :rotation-gain 2.0 for better motion.
however it depends on rotation angle and hardware situation.

Reference:
https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/fetcheus/fetch-interface.l#L241-L323

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

by the way, we checked gas spring of fetch yesterday with @708yamaguchi and @HiroIshida, and we need to replace it.

(let* ((msec (* diff-len 10))
(x (/ (elt (send diff :worldpos) 0) msec))
(y (/ (elt (send diff :worldpos) 1) msec))
Expand All @@ -1436,6 +1440,28 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(return-from :move-to-wait nil)))
;;(unix::usleep (* (round msec) 1000)) ;; why time wait
)
(let* ((x (elt (send diff :worldpos) 0))
(y (elt (send diff :worldpos) 1))
(forward-p (>= x 0))
(d (elt (car (rpy-angle (send diff :worldrot))) 0))
(d0 (if forward-p (atan2 y x) (atan2 (- y) (- x))))
(d1 (- d d0))
(dv0-msec (* (abs d0) 1000))
(dv0 (/ d0 (/ dv0-msec 1000.0)))
(v-msec (* diff-len 10))
(v (* (if forward-p 1.0 -1.0) (/ diff-len v-msec)))
(dv1-msec (* (abs d1) 1000))
(dv1 (/ d1 (/ dv1-msec 1000.0))))
(ros::ros-warn ":move-to -> :go-velocity x:0 y:0 d:~A msec:~A" dv0 dv0-msec)
(ros::ros-warn ":move-to -> :go-velocity x:~A y:0 d:0 msec:~A" v v-msec)
(ros::ros-warn ":move-to -> :go-velocity x:0 y:0 d:~A msec:~A" dv1 dv1-msec)
(unix:usleep (* 400 1000)) ;; 400ms ???
(let ((acret (and (send self :go-velocity 0 0 dv0 dv0-msec :wait t)
(send self :go-velocity v 0 0 v-msec :wait t)
(send self :go-velocity 0 0 dv1 dv1-msec :wait t))))
(unless acret
(setq move-base-goal-msg nil)
(return-from :move-to-wait nil)))))
(progn
(ros::ros-error "too far from goal position ~A mm (> 200mm)" diff-len)
;; move-to succeeded but away from 200 mm
Expand Down