Skip to content

Commit ae09428

Browse files
committed
[euslisp/rtm-ros-robot-interface.l] Update codes, documentation strings, and warning message to make :off-xx-vector deprecated.
1 parent 2e664c6 commit ae09428

File tree

1 file changed

+12
-8
lines changed

1 file changed

+12
-8
lines changed

hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -104,15 +104,19 @@
104104
(send self :tmp-force-moment-vector :moment limb))
105105
(:off-force-vector
106106
(&optional (limb))
107-
"Returns offset-removed :force-vector [N] list for all limbs obtained by :state.
107+
"!! This method is deprecated, use :force-vector !!
108+
Returns offset-removed :force-vector [N] list for all limbs obtained by :state.
108109
This value corresponds to RemoveForceSensorLinkOffset RTC.
109110
If a limb argument is specified, returns a vector for the limb."
111+
(warning-message 1 ";; !!~%;; !! :off-force-vector is deprecated, use :force-vector !!~%;; !!~%")
110112
(send self :tmp-force-moment-vector :force limb "off"))
111113
(:off-moment-vector
112114
(&optional (limb))
113-
"Returns offset-removed :moment-vector [Nm] list for all limbs obtained by :state.
115+
"!! This method is deprecated, use :moment-vector !!
116+
Returns offset-removed :moment-vector [Nm] list for all limbs obtained by :state.
114117
This value corresponds to RemoveForceSensorLinkOffset RTC.
115118
If a limb argument is specified, returns a vector for the limb."
119+
(warning-message 1 ";; !!~%;; !! :off-moment-vector is deprecated, use :moment-vector !!~%;; !!~%")
116120
(send self :tmp-force-moment-vector :moment limb "off"))
117121
(:reference-force-vector
118122
(&optional (limb))
@@ -132,20 +136,20 @@
132136
This value corresponds to RemoveForceSensorLinkOffset RTC.
133137
If a limb argument is specified, returns a vector for the limb."
134138
(if limb
135-
(send (car (send robot limb :force-sensors)) :rotate-vector (send self :off-force-vector limb))
139+
(send (car (send robot limb :force-sensors)) :rotate-vector (send self :force-vector limb))
136140
(mapcar #'(lambda (fs force)
137141
(send fs :rotate-vector force))
138-
(send robot :force-sensors) (send self :off-force-vector))))
142+
(send robot :force-sensors) (send self :force-vector))))
139143
(:absolute-moment-vector
140144
(&optional (limb))
141145
"Returns offset-removed :moment-vector [Nm] list for all limbs in world frame obtained by :state.
142146
This value corresponds to RemoveForceSensorLinkOffset RTC.
143147
If a limb argument is specified, returns a vector for the limb."
144148
(if limb
145-
(send (car (send robot limb :force-sensors)) :rotate-vector (send self :off-moment-vector limb))
149+
(send (car (send robot limb :force-sensors)) :rotate-vector (send self :moment-vector limb))
146150
(mapcar #'(lambda (fs moment)
147151
(send fs :rotate-vector moment))
148-
(send robot :force-sensors) (send self :off-moment-vector))))
152+
(send robot :force-sensors) (send self :moment-vector))))
149153
(:zmp-vector
150154
(&optional (wrt :local))
151155
"Returns zmp vector [mm].
@@ -1053,12 +1057,12 @@
10531057
(let* ((params (mapcar #'(lambda (alimb) (send self :get-forcemoment-offset-param alimb)) limbs)))
10541058
(labels ((calc-off
10551059
(alimb)
1056-
(send self (if (eq f/m :force) :off-force-vector :off-moment-vector) alimb))
1060+
(send self (if (eq f/m :force) :force-vector :moment-vector) alimb))
10571061
(get-avg-fm
10581062
()
10591063
(let ((fm (mapcar #'(lambda (i)
10601064
(send self :state)
1061-
(mapcar #'(lambda (alimb) (send self (if (eq f/m :force) :off-force-vector :off-moment-vector) alimb)) limbs))
1065+
(mapcar #'(lambda (alimb) (send self (if (eq f/m :force) :force-vector :moment-vector) alimb)) limbs))
10621066
(make-list itr))))
10631067
(mapcar #'(lambda (alimb)
10641068
(let ((idx (position alimb limbs)))

0 commit comments

Comments
 (0)