Merge branch 'master' into use-ec-interpolation

jsk-ros-pkg · Feb 7, 2017 · ef0579c · ef0579c
2 parents 1366a37 + 968d9c5
commit ef0579c
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 2 deletions.
diff --git a/jsk_baxter_robot/baxtereus/baxter-util.l b/jsk_baxter_robot/baxtereus/baxter-util.l
@@ -1,6 +1,13 @@
 (require :baxter "package://baxtereus/baxter.l")
 
 (defmethod baxter-robot
+  (:ik-prepared-poses
+   (&optional (poses :null))
+   (let ((ik-prepared-poses (or (get self :ik-prepared-poses) '(:untuck-pose))))
+     (if (eq poses :null)
+         (return-from :ik-prepared-poses ik-prepared-poses)
+       (setf (get self :ik-prepared-poses) poses))
+     poses))
   (:inverse-kinematics
    (target-coords &rest args &key (avoid-collision-distance 5) &allow-other-keys)
    "Compute Inverse Kinematics with some strategies.
@@ -34,7 +41,7 @@
        (format *error-output* "; failed for slow ik, try to start from prepared poses~%")
        (let* ((move-joints (send-all (cadr (memq :link-list args)) :joint))
               (av (mapcar #'(lambda (j) (send j :joint-angle)) (send self :joint-list)))
-              (ik-prepared-poses (if (memq :ik-prepared-poses (send self :methods)) (send self :ik-prepared-poses) '(:untuck-pose))))
+              (ik-prepared-poses (send self :ik-prepared-poses)))
          (dolist (pose ik-prepared-poses)
            (unless r
              (format *error-output* "; starting from prepared pose '~A'~%" pose)
@@ -99,7 +106,7 @@
 	 (send (send self :right_e0) :max-joint-velocity 1)
 	 (send (send self :right_s0) :max-joint-velocity 1.4)
          )
-  (:self-collision-check (&key (pairs (send self :collision-check-pairs :links collision-avoidance-links)) &rest args &allow-other-keys)
+  (:self-collision-check (&rest args &key (pairs (send self :collision-check-pairs :links collision-avoidance-links)) &allow-other-keys)
                          (send-super* :self-collision-check :pairs pairs args))
   )
 
diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/warning.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/warning.py
@@ -69,6 +69,7 @@ def __init__(self):
         self.diagnostics_status_sub = rospy.Subscriber("diagnostics", DiagnosticArray, self.diagnostics_status_callback, queue_size = 1)
         self.base_breaker = rospy.ServiceProxy('base_breaker', BreakerCommand)
         #
+        self.robot_state_msgs = RobotState()
         self.battery_state_msgs = BatteryState()
         self.twist_msgs = Twist()
         #