#! /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)