iory

echo ";; start checking https://github.com/ros/geometry.git"
for package in eigen_conversions kdl_conversions tf tf_conversions; do
    echo ";; ==============================================="
    echo ";; check package $package"
    mkdir -p /tmp/ros/$package/src
    cd /tmp/ros/$package/src
    rosinstall_generator $package --rosdistro melodic --deps > .rosinstall
    wstool up > /dev/null 2>&1
    grep --include CMakeLists.txt "^[^#;].*python.*" -n -i -r | grep -v catkin_python_setup | grep -v catkin_install_python
done

iory

(defun counter-clockwise-angle-between-vectors (v1 v2 normal-vector)
  (let* ((det (v. normal-vector (v* v1 v2)))
         (dot (v. v1 v2))
         (angle (+ (atan2 (- det) (- dot)) pi)))
    (when (eps= angle 2pi)
      (setq angle 0.0))
    angle))


(setq v1 #f(1 0 0))

iory

(load "package://pr2eus/robot-interface.l")
(load "package://pr2eus/pr2-interface.l")
(load "package://jsk_maps/src/eng2-scene.l")
(pr2-init)
(setq *robot* *pr2*)
(setq *scene* (make-eng2-scene))
(pprint (send-all (send *scene* :spots) :name))

(send *ri* :move-to
      (send *scene* :spot  "/eng2/7f/room73B2-door-front"))

iory

;; hrp2jsknts

(defun knee-standing ()

  (send *robot* :reset-manip-pose)
  (setq larm-reset-manip-pose (send *robot* :larm :angle-vector))
  (setq rarm-reset-manip-pose (send *robot* :rarm :angle-vector))

  (send *robot* :reset-pose)
  (send *robot* :legs :move-end-pos #f(0 0 360))

iory

#!/usr/bin/env roseus

(ros::roseus "tf_sample_program")

(defun setup ()
  (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l")
  (hrp2jsknts-init)
  (setq *robot* *hrp2jsknts*)
  (send *robot* :fix-leg-to-coords (make-coords))
  (objects (list *robot*))

iory

import argparse
import sys
import numpy as np
sys.path.append("../..")
from py3d import *


parser = argparse.ArgumentParser()
parser.add_argument('filename', type=str)
parser.add_argument('-v', '--voxel-size', type=int,

iory

import sys
import numpy as np
sys.path.append("../..")
from py3d import *


print("Load a ply point cloud, print it, and render it")
pcd = read_point_cloud("/tmp/screw.ply")
print(pcd)
print(np.asarray(pcd.points))

iory

!/usr/bin/env roseus

(defmethod coordinates
  (:draw-on2
    (&key ((:viewer vwer) user::*viewer*)
          flush (width (get self :width))
          (size (get self :size)))
    (if flush (send vwer :viewsurface :makecurrent))
    (let ((pwidth (send vwer :viewsurface :line-width))
          (pcolor (send vwer :viewsurface :color))

iory

import numpy as np
import matplotlib  # NOQA
import matplotlib.pyplot as plt
import pybullet as p
from pybullet_envs.bullet.kukaCamGymEnv import KukaCamGymEnv
env = KukaCamGymEnv(renders=True)

ob = env.reset()

for i in range(0, 180, 5):

iory

#!/usr/bin/env roseus

;; write
(with-open-file
 (launch-f "./hoge.txt" :direction :output)
 (dotimes (n 10)
   (format launch-f "~A~%" n)))

;; if you want to append
(with-open-file