#! /usr/bin/python -i
# -*- coding: utf-8 -*-
"""@package demo
ロボット制御クラスライブラリをpythonから利用するサンプル
a sample python script using "class library to control hiro"
"""
import KSP
robot = KSP.Robot("192.168.128.129")
rbt = robot
arm = KSP.dblArray(KSP.DOFS_ARM)
# for robot.[rl]arm.getAngles()
whole = KSP.dblArray(KSP.DOFS_WHOLE)
# for robot.getAngles()
def jointCalib():
robot.jointCalib()
def goInit():
""" @brief ロボットを初期姿勢まで動かす """
robot.makePose(0, 0, 0, -0.6, 0, -100, 15.2, 9.4, 3.2, 0.6, 0, -100, -15.2,
9.4, -3.2, 10)
return
def goEscape():
## @ロボットをエスケープ姿勢まで動かす
from sensor_msgs.msg import JointState
from hiro_control.srv import *
import math
import sys
sys.path.append(
rospy.get_param('/hiro_driver_path',
'/home/vektor/4/ros-pkg/hiro_control/driver/hiro'))
#rospy.loginfo('%s', sys.path)
import KSP
hiro = KSP.Robot(rospy.get_param('/hiro_ip_addr', '192.168.128.129'))
def monitor_joint_state():
raw_chest_joint_state = KSP.dblArray(1)
raw_head_joint_states = KSP.dblArray(2)
raw_rarm_joint_states = KSP.dblArray(6)
hiro.getChestAngle(raw_chest_joint_state)
raw_chest_joint_state[0] = math.radians(raw_chest_joint_state[0])
hiro.getNeckAngles(raw_head_joint_states)
for i in range(2):
raw_head_joint_states[i] = math.radians(raw_head_joint_states[i])
hiro.rarm.getAngles(raw_rarm_joint_states)