Robot interface

controller-action-client

:super ros::simple-action-client
:slots time-to-finish last-feedback-msg-stamp ri angle-vector-sequence timer-sequence current-time current-angle-vector previous-angle-vector scale-angle-vector

:init r &rest args

:last-feedback-msg-stamp

:time-to-finish

:action-feedback-cb msg

:push-angle-vector-simulation av tm &optional prev-av

:pop-angle-vector-simulation

:interpolatingp

robot-interface

:super propertied-object
:slots robot objects robot-state joint-action-enable warningp controller-type controller-actions controller-timeout namespace controller-table visualization-topic simulation-default-look-all-p joint-states-topic pub-joint-states-topic viewer groupname

robot-interface is object for interacting real robot thorugh JointTrajectoryAction servers and JointState topics, this function uses old nervous-style interface for histrical reason. If JointTrajectoryAcion serve is not found, this instance runs as simulation mode, see "Kinematics Simulator" view as simulatied robot,

    (setq *ri* (instance robot-interface :init))
    (send *ri* :angle-vector (send *robot* :joint-angle) 2000)
    (send *ri* :wait-interpolation)
    (send *ri* :state :potentio-vector)

:init

   &rest args &key ((:robot r))
   ((:objects objs))
   (type :default-controller)
   (use-tf2)
   ((:groupname nh) robot_multi_queue)
   ((:namespace ns))
   ((:joint-states-topic jst) joint_states)
   ((:joint-states-queue-size jsq) 1)
   ((:publish-joint-states-topic pjst) nil)
   ((:controller-timeout ct) 3)
   ((:visuzlization-marker-topic vmt) robot_interface_marker_array)
   ((:simulation-look-all sim-look-all) t)
   &allow-other-keys

Create robot interface
robot : class name of robot
objects : objects to be displayed in simulation (only for simulation)
type : method name for defineing controller type
use-tf2 : use tf2
groupname : ros nodehandle group name
namespace : ros nodehandle name space
joint-states-topic jst : name for subscribing joint state topic
joint-states-queue-size : queue size of joint state topic. if you have two different node publishing joint_state, better to use 2. default is 1
publish-joint-state-topic : name for publishing joint state topic (only for simulation)
conter-timeout : timeout seconds for controller lookup
visualizatoin-marker-topic : topic name for visualize

:angle-vector-safe

   av tm &key (simulation)
   (yes)
   (wait t)
   (norm-threshold 120)
   (angle-threshold 80)
   (velocity-threshold 240)

send joint angle to robot with user-level confirmation. This method requires key input

:angle-vector

   av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1)
   (min-time 1.0)
   (end-coords-interpolation nil)

Send joind angle to robot, this method retuns immediately, so use :wait-interpolation to block until the motion stops.
av : joint angle vector [rad]
tm : (time to goal in [msec])
if designated tm is faster than fastest speed, use fastest speed
if not specified, it will use 1/scale of the fastest speed .
if :fastest is specefied use fastest speed calcurated from max speed
ctype : controller method name
start-time : time to start moving
scale : if tm is not specified, it will use 1/scale of the fastest speed
min-time : minimum time to for time to goal
end-coords-interpolation : set t if you want to move robot in cartesian space interpolation

:angle-vector-sequence

   avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1)
   (min-time 0.0)
   (end-coords-interpolation nil)

Send joind angle to robot, this method retuns immediately, so use :wait-interpolation to block until the motion stops.

:wait-interpolation

&optional (ctype) (timeout 0)

Wait until last sent motion is finished
ctype : controller to be wait
timeout : max time of for waiting
return values is a list of interpolatingp for all controllers, so (null (some #'identity (send ri :wait-interpolation))) -> t if all interpolation has stopped

:interpolatingp

&optional (ctype)

Check if the last sent motion is executing
return t if interpolating

:wait-interpolation-smooth

time-to-finish &optional (ctype)

Return time-to-finish [msec] before the sent command is finished. Example code are:
```
(dolist (av (list av1 av2 av3 av4))
    (send *ri* :angle-vector av)
    (send *ri* :wait-interpolation-smooth 300))
```
Return value is a list of interpolatingp for all controllers, so (null (some #'identity (send ri :wait-interpolation))) -> t if all interpolation has stopped

:interpolating-smoothp

time-to-finish &optional (ctype)

Check inf the last sent motion will stops for next time-to-finish [msec]

:stop-motion

&key (stop-time 0)

Stop current executing motion

:cancel-angle-vector

   &key ((:controller-actions ca) controller-actions)
   ((:controller-type ct) controller-type)
   (wait)

Cancerl current joint trajetory action

:worldcoords

Returns world coords of robot, This method uses caced data

:torque-vector

Return torque vector of robot, This method uses caced data

:potentio-vector

Retuns current robot angle vector, This method uses caced data

:reference-vector

Return reference joint angle vector, This method uread current state.

:actual-vector

Returns current robot angle vector, This method read curren tstate.

:error-vector

Returns error vector of the robot, This method read current state.

:state

&rest args

Read robot state and update internal robot instance
:wait-until-update nil wait until joint_state is updated

:find-object

name-or-obj

Returns objects with given name or object of the same name.

:simulation-modep

Check if simulation mode

:warningp

&optional (w :dummy)

When warning mode, it wait for user's key input before sending angle-vector to the robot

:go-pos

x y &optional (d 0)

move robot toward x, y, degree and wait to reach that goal. return t if reached or nil if fail
the robot moves relative to current position.
using [m] and [degree] is historical reason from original hrpsys code

:go-pos-no-wait

x y &optional (d 0)

no-wait version of :go-pos. this function is always assumed to return t

:go-wait

wait until :go-pos-no-wait reached to the goal, return t if reached or nil if fail

:go-velocity

x y d &optional (msec 1000) &key (stop t)
(wait)

move robot at given speed for given msec.
if stop is t, robot stops after msec, use wait t for blocking call
return nil if aborted while waiting by enabling :wait, otherwise return t

:go-stop

stop go-velocity. return t if robot successfly stops, otherwise return nil

:gripper

&rest args

get information about gripper

:nod

&key (angle 40)
(time 3000)

Nod robot head

:eus-mannequin-mode

tmp-robot limb &key ((:time tm) 1000)
((:viewer vwr) viewer)

Euslisp version mannequin mode.
In every loop, :angle-vector is continuously updated.
If you want to stop this mode, press Enter.
tmp-robot : argument for robot instance and angle-vector of tmp-robot is updated in this method.
limb : mannequin mode limb such as :rarm, :larm, :rleg, and :lleg.
tm : angle-vector update time [ms]. 1000 by default.

:add-controller ctype &key (joint-enable-check) (create-actions)

:robot-interface-simulation-callback

:publish-joint-state &optional (joint-list (send robot :joint-list))

:angle-vector-duration start end &optional (scale 1) (min-time 1.0) (ctype controller-type)

:angle-vector-simulation av tm ctype

:state-vector type &key ((:controller-actions ca) controller-actions) ((:controller-type ct) controller-type)

:send-ros-controller action joint-names starttime trajpoints

:set-robot-state1 key msg

:ros-state-callback msg

:wait-until-update-all-joints tgt-tm

:update-robot-state &key (wait-until-update nil)

:default-controller

:sub-angle-vector v0 v1

:robot &rest args

:viewer &rest args

:objects &optional objs

:set-simulation-default-look-at look-at-p

:draw-objects &key look-all

:joint-action-enable &optional (e :dummy)

:spin-once

:send-trajectory joint-trajectory-msg &key ((:controller-actions ca) controller-actions) ((:controller-type ct) controller-type) (starttime 1) &allow-other-keys

:send-trajectory-each action joint-names traj &optional (starttime 0.2)

:ros-wait tm &key (spin) (spin-self) (finish-check) &allow-other-keys

:joint-trajectory-to-angle-vector-list move-arm joint-trajectory &key ((:diff-sum diff-sum) 0) ((:diff-thre diff-thre) 50) (show-trajectory t) (send-trajectory t) ((:speed-scale speed-scale) 1.0) &allow-other-keys

:show-goal-hand-coords coords move-arm

:find-descendants-dae-links l

:show-mesh-traj-with-color link-body-list link-coords-list &key ((:lifetime lf) 20) (ns robot_traj) ((:color col) #f(0.5 0.5 0.5))

robot-move-base-interface

:super robot-interface
:slots move-base-action move-base-trajectory-action move-base-goal-msg move-base-goal-coords move-base-goal-map-to-frame base-frame-id odom-topic go-pos-unsafe-goal-msg current-goal-coords

:move-trajectory-sequence

   trajectory-points time-list &key (stop t)
   (start-time)
   (send-action nil)

trajectory-points [ list of #f(x y d) ] time-list [list of time span]
stop [ stop after msec moveing ]
start-time [ robot will move at start-time ]
send-action [ send message to action server, it means robot will move ]

:move-trajectory

   x y d &optional (msec 1000) &key (stop t)
   (start-time)
   (send-action nil)

x [m/sec] y [m/sec] d [rad/sec] msec [milli second]
stop [ stop after msec moveing ]
start-time [ robot will move at start-time ]
send-action [ send message to action server, it means robot will move ]

: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) ((:odom-topic odom-topic-name) /base_odometry/odom) &allow-other-keys

:odom-callback msg

:go-stop &optional (force-stop t)

:make-plan st-cds goal-cds &key (start-frame-id /world) (goal-frame-id /world)

:move-to coords &rest args &key (no-wait nil) &allow-other-keys

:move-to-send coords &key (frame-id /world) (wait-for-server-timeout 5) (count 0) &allow-other-keys

:move-to-wait &rest args &key (retry 10) (frame-id /world) &allow-other-keys

:go-waitp

:go-pos x y &optional (d 0)

:go-pos-no-wait x y &optional (d 0)

:go-wait

:go-velocity x y d &optional (msec 1000) &key (stop t) (wait)

:go-pos-unsafe &rest args

:go-pos-unsafe-no-wait x y &optional (d 0)

:go-pos-unsafe-wait

:state &rest args

ros-interface

:super robot-interface
:slots nil

:init &rest args

make-robot-interface-from-name

name &rest args

make a robot model from string: (make-robot-model "pr2")

init-robot-from-name

robot-name &rest args

call ${robot}-init function

clear-costmap

&key (node-name /move_base_node)

reset local costmap and clear unknown grid around robot

change-inflation-range

   &optional (range 0.2) &key (node-name /move_base_node)
   (costmap-name local_costmap)
   (inflation-name inflation)

change inflation range of local costmap

robot-init

&optional (robot-name (ros::get-param /robot/type))

Initialize robot-model and robot-interface instances respectively, such as pr2 and ri.
Behavior:
1. Without /robot/type and argument, or no interface file is found, robot instance and interface instance are not created and errors are invoked.
2. If argument are specified and robot interface file is found, robot instance and interface with given name are created.
3. If no argument is specified, /robot/type ROSPARAM is set, and robot interface file is found, robot instance and interface with given name are created.
  Typical usage:
  (robot-init) ;; With setting /robot/type ROSPARAM anywhere.
  (robot-init (or (ros::get-param "/robot/type") "pr2")) ;; If /robot/type ROSPARAM is specified, use it. Otherwise, use "pr2" by default.
  Configuring user-defined robot:
  This function searches robot interface file from rospack plugins.
  If user want to use their own robots from robot-init function,
  please write export tag in [user_defined_rospackage]/package.xml as pr2eus/package.xml.

joint-list->joint_state jlist &key (position) (effort 0) (velocity 0)

apply-joint_state jointstate robot

apply-trajectory_point names trajpoint robot

apply-joint_trajectory joint-trajectory robot &optional (offset 200.0)