def test0():
robot.rhand.setDefaultComplementType(KSP.COMPLEMENTBY_LINE)
robot.lhand.setDefaultComplementType(KSP.COMPLEMENTBY_LINE)
rxyz = KSP.dblArray(3)
lxyz = KSP.dblArray(3)
rxyz[0] = 300
lxyz[0] = 300
rxyz[1] = -200
lxyz[1] = 200
rxyz[2] = 200
lxyz[2] = 200
robot.rhand.moveTo(rxyz)
robot.lhand.moveTo(lxyz)
robot.wait()
for i in range(3):
robot.rhand.move(0, 0, 50, 20, KSP.COMPLEMENTBY_ANGLE)
robot.lhand.move(0, 0, 50, 20, KSP.COMPLEMENTBY_ANGLE)
robot.wait()
robot.rhand.moveTo(rxyz)
robot.lhand.moveTo(lxyz)
robot.wait()
def test0():
robot.rhand.setDefaultComplementType(KSP.COMPLEMENTBY_LINE)
robot.lhand.setDefaultComplementType(KSP.COMPLEMENTBY_LINE)
rxyz = KSP.dblArray(3)
lxyz = KSP.dblArray(3)
rxyz[0] = 300
lxyz[0] = 300
rxyz[1] = -200
lxyz[1] = 200
rxyz[2] = 200
lxyz[2] = 200
robot.rhand.moveTo(rxyz)
robot.lhand.moveTo(lxyz)
robot.wait()
for i in range(3):
robot.rhand.move(0, 0, 50, 20, KSP.COMPLEMENTBY_ANGLE)
robot.lhand.move(0, 0, 50, 20, KSP.COMPLEMENTBY_ANGLE)
robot.wait()
robot.rhand.moveTo(rxyz)
robot.lhand.moveTo(lxyz)
robot.wait()
def __init__(self):
""" コンストラクタ """
self.robot = KSP.Robot("192.168.128.3")
self.neck = KSP.dblArray(2)
self.move = False
## ウィンドウ作成
cv.NamedWindow( "CamShiftDemo", 1 )
cv.NamedWindow( "Histogram", 1 )
cv.SetMouseCallback( "CamShiftDemo", self.on_mouse)
cv.CreateTrackbar("Vmin", "CamShiftDemo", self.vmin, 256, self.track_vmin)
cv.CreateTrackbar("Vmax", "CamShiftDemo", self.vmax, 256, self.track_vmax)
cv.CreateTrackbar("Smin", "CamShiftDemo", self.smin, 256, self.track_smin)
## キャプチャパラメータ設定
self.cams = [0]
self.width, self.height = 320, 240
self.cap = uc.CaptureFromUEYE()
uc.SetBinning(self.cap, self.cams, (uc.IS_BINNING_4X_VERTICAL, uc.IS_BINNING_4X_HORIZONTAL))
uc.SetImageAOI(self.cap, self.cams, (160, 120, self.width, self.height))
#uc.SetAutoParameter(self.cap, self.cams, (uc.IS_SET_ENABLE_AUTO_GAIN, 1.0, 0.0))
#uc.SetAutoParameter(self.cap, self.cams, (uc.IS_SET_ENABLE_AUTO_SHUTTER, 1.0, 0.0))
#uc.SetAutoParameter(self.cap, self.cams, (uc.IS_SET_AUTO_WB_ONCE, 1.0, 0.0))
uc.SetAutoParameter(self.cap, self.cams, (uc.IS_SET_ENABLE_AUTO_WHITEBALANCE, 1.0, 0.0))
print( "Keys:\n"
" ESC - quit the program\n"
" b - switch to/from backprojection view\n"
"To initialize tracking, drag across the object with the mouse\n" )
def handle_control_rarm(req):
#def handle_control_rarm():
# # Wacth out the order + should be in DEGREE
# hiro.makePose(0.,# joint_chest_yaw
# 0.,0.,# Head
# -0.6, 0., -100., 15.2, 9.4, 3.2,# right arm values
# -9.0, -154.8, -149.9, -116.1, -21.8, -10.0, # left arm joint values (IN DEGREE), which is the HOME_POSE values
# 10)
# Wacth out the order + should be in DEGREE
hiro.makePose(math.degrees(req.desired.positions[0]),# joint_chest_yaw
0.,0.,# Head
math.degrees(req.desired.positions[1]), math.degrees(req.desired.positions[2]), math.degrees(req.desired.positions[3]), math.degrees(req.desired.positions[4]), math.degrees(req.desired.positions[5]), math.degrees(req.desired.positions[6]),# right arm values
-9.0, -154.8, -149.9, -116.1, -21.8, -10.0, # left arm joint values (IN DEGREE), which is the HOME_POSE values
20)# Speed; Prev (integer): 10; 20; 25
hiro.wait();# Wait until the currently running operation finished
# Sense the results, should wait until makePose above is finished
chest_joint_state = KSP.dblArray(1)
rarm_joint_state = KSP.dblArray(6)
hiro.getChestAngle(chest_joint_state)
chest_joint_state[0] = math.radians(chest_joint_state[0])
hiro.rarm.getAngles(rarm_joint_state)
for i in range(6):
rarm_joint_state[i] = math.radians(rarm_joint_state[i])
# Wrap the response
actual_positions = [chest_joint_state[0], rarm_joint_state[0], rarm_joint_state[1], rarm_joint_state[2], rarm_joint_state[3], rarm_joint_state[4], rarm_joint_state[5]]# Actually, this is rarm group that contains rarm joints and a chest joint
actual_velocities = [req.desired.velocities[0], req.desired.velocities[1], req.desired.velocities[2], req.desired.velocities[3], req.desired.velocities[4], req.desired.velocities[5], req.desired.velocities[6]]# Up to now, just as the same as the req
actual_accelerations = [req.desired.accelerations[0], req.desired.accelerations[1], req.desired.accelerations[2], req.desired.accelerations[3], req.desired.accelerations[4], req.desired.accelerations[5], req.desired.accelerations[6]]# Up to now, just as the same as the req
actual_time_from_start = rospy.Duration(0.5)# This is useless so far
res = trajectory_msgs.msg.JointTrajectoryPoint(actual_positions, actual_velocities, actual_accelerations, actual_time_from_start)
return ControlArmResponse(res)
#! /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():
## @ロボットをエスケープ姿勢まで動かす
#! /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():
## @ロボットをエスケープ姿勢まで動かす
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)
for i in range(6):
raw_rarm_joint_states[i] = math.radians(raw_rarm_joint_states[i])
# # FOR TESTING PURPOSE, should not coexist with the real one
# raw_chest_joint_state[0] = math.radians(0.)# For the torso, including the chest
#
# raw_head_joint_state[0] = math.radians(0.)# For the head, [0] is yaw/pan,
# raw_head_joint_state[1] = math.radians(0.)# For the head, [1] is pitch/tilt
#
# raw_rarm_joint_state[0] = math.radians(0.)# joint_rshoulder_yaw
# raw_rarm_joint_state[1] = math.radians(0.)# joint_rshoulder_pitch
# raw_rarm_joint_state[2] = math.radians(0.)# joint_relbow_pitch
# raw_rarm_joint_state[3] = math.radians(0.)# joint_rwrist_yaw
# raw_rarm_joint_state[4] = math.radians(0.)# joint_rwrist_pitch
# raw_rarm_joint_state[5] = math.radians(0.)# joint_rwrist_roll
joint_states = JointState()
# RARM joints
joint_states.header.stamp = rospy.Time.now()
joint_states.header.frame_id = '/none'
joint_states.name.append('joint_chest_yaw')
joint_states.position.append(raw_chest_joint_state[0])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rshoulder_yaw')
joint_states.position.append(raw_rarm_joint_states[0])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rshoulder_pitch')
joint_states.position.append(raw_rarm_joint_states[1])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_relbow_pitch')
joint_states.position.append(raw_rarm_joint_states[2])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rwrist_yaw')
joint_states.position.append(raw_rarm_joint_states[3])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rwrist_pitch')
joint_states.position.append(raw_rarm_joint_states[4])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rwrist_roll')
joint_states.position.append(raw_rarm_joint_states[5])
joint_states.velocity.append(0)
joint_states.effort.append(0)
# Head joints
joint_states.name.append('joint_head_yaw')
joint_states.position.append(raw_head_joint_states[0])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_head_pitch')
joint_states.position.append(raw_head_joint_states[1])
joint_states.velocity.append(0)
joint_states.effort.append(0)
# Publish
joint_states_pub = rospy.Publisher("joint_states", JointState)
joint_states_pub.publish(joint_states)
#! /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.3");
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():
## @ロボットをエスケープ姿勢まで動かす
robot.makeEscapePose(10);
return ;
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)
for i in range(6):
raw_rarm_joint_states[i] = math.radians(raw_rarm_joint_states[i])
# # FOR TESTING PURPOSE, should not coexist with the real one
# raw_chest_joint_state[0] = math.radians(0.)# For the torso, including the chest
#
# raw_head_joint_state[0] = math.radians(0.)# For the head, [0] is yaw/pan,
# raw_head_joint_state[1] = math.radians(0.)# For the head, [1] is pitch/tilt
#
# raw_rarm_joint_state[0] = math.radians(0.)# joint_rshoulder_yaw
# raw_rarm_joint_state[1] = math.radians(0.)# joint_rshoulder_pitch
# raw_rarm_joint_state[2] = math.radians(0.)# joint_relbow_pitch
# raw_rarm_joint_state[3] = math.radians(0.)# joint_rwrist_yaw
# raw_rarm_joint_state[4] = math.radians(0.)# joint_rwrist_pitch
# raw_rarm_joint_state[5] = math.radians(0.)# joint_rwrist_roll
joint_states = JointState()
# RARM joints
joint_states.header.stamp = rospy.Time.now()
joint_states.header.frame_id = '/none'
joint_states.name.append('joint_chest_yaw')
joint_states.position.append(raw_chest_joint_state[0])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rshoulder_yaw')
joint_states.position.append(raw_rarm_joint_states[0])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rshoulder_pitch')
joint_states.position.append(raw_rarm_joint_states[1])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_relbow_pitch')
joint_states.position.append(raw_rarm_joint_states[2])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rwrist_yaw')
joint_states.position.append(raw_rarm_joint_states[3])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rwrist_pitch')
joint_states.position.append(raw_rarm_joint_states[4])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_rwrist_roll')
joint_states.position.append(raw_rarm_joint_states[5])
joint_states.velocity.append(0)
joint_states.effort.append(0)
# Head joints
joint_states.name.append('joint_head_yaw')
joint_states.position.append(raw_head_joint_states[0])
joint_states.velocity.append(0)
joint_states.effort.append(0)
joint_states.name.append('joint_head_pitch')
joint_states.position.append(raw_head_joint_states[1])
joint_states.velocity.append(0)
joint_states.effort.append(0)
# Publish
joint_states_pub = rospy.Publisher("joint_states", JointState)
joint_states_pub.publish(joint_states)
def handle_control_rarm(req):
#def handle_control_rarm():
# # Wacth out the order + should be in DEGREE
# hiro.makePose(0.,# joint_chest_yaw
# 0.,0.,# Head
# -0.6, 0., -100., 15.2, 9.4, 3.2,# right arm values
# -9.0, -154.8, -149.9, -116.1, -21.8, -10.0, # left arm joint values (IN DEGREE), which is the HOME_POSE values
# 10)
# Wacth out the order + should be in DEGREE
hiro.makePose(
math.degrees(req.desired.positions[0]), # joint_chest_yaw
0.,
0., # Head
math.degrees(req.desired.positions[1]),
math.degrees(req.desired.positions[2]),
math.degrees(req.desired.positions[3]),
math.degrees(req.desired.positions[4]),
math.degrees(req.desired.positions[5]),
math.degrees(req.desired.positions[6]), # right arm values
-9.0,
-154.8,
-149.9,
-116.1,
-21.8,
-10.0, # left arm joint values (IN DEGREE), which is the HOME_POSE values
20) # Speed; Prev (integer): 10; 20; 25
hiro.wait()
# Wait until the currently running operation finished
# Sense the results, should wait until makePose above is finished
chest_joint_state = KSP.dblArray(1)
rarm_joint_state = KSP.dblArray(6)
hiro.getChestAngle(chest_joint_state)
chest_joint_state[0] = math.radians(chest_joint_state[0])
hiro.rarm.getAngles(rarm_joint_state)
for i in range(6):
rarm_joint_state[i] = math.radians(rarm_joint_state[i])
# Wrap the response
actual_positions = [
chest_joint_state[0], rarm_joint_state[0], rarm_joint_state[1],
rarm_joint_state[2], rarm_joint_state[3], rarm_joint_state[4],
rarm_joint_state[5]
] # Actually, this is rarm group that contains rarm joints and a chest joint
actual_velocities = [
req.desired.velocities[0], req.desired.velocities[1],
req.desired.velocities[2], req.desired.velocities[3],
req.desired.velocities[4], req.desired.velocities[5],
req.desired.velocities[6]
] # Up to now, just as the same as the req
actual_accelerations = [
req.desired.accelerations[0], req.desired.accelerations[1],
req.desired.accelerations[2], req.desired.accelerations[3],
req.desired.accelerations[4], req.desired.accelerations[5],
req.desired.accelerations[6]
] # Up to now, just as the same as the req
actual_time_from_start = rospy.Duration(0.5) # This is useless so far
res = trajectory_msgs.msg.JointTrajectoryPoint(actual_positions,
actual_velocities,
actual_accelerations,
actual_time_from_start)
return ControlArmResponse(res)
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)
- how to handle the algorithm's output
Created on June 5, 2014 by Gabriel de Oliveira Ramos <*****@*****.**>
'''
#import the KSP source
import KSP
# parameters to be passed to the KSP algorithm
graph_file = 'OW.net' # the graph of the traffic network (the file format is specified by the algorithm's help)
ODpairs = ['A|L', 'B|M'] # the list of origins and destinations
K = 4 # the number of paths to find
flow = 0.0 # the flow of vehicles to be used when computing the links' costs (the default is zero)
# generate the list of vertices and edges from the network file
V, E, OD = KSP.generateGraph(graph_file, flow)
# for each OD pair
for od in ODpairs: # to look at all pairs, use the variable OD (above)
#~ print 'Pair %s' % od
origin, destination = od.split('|')
# run the algorithm (return the K routes and associated costs of the given origin-destination pair)
routes = KSP.getKRoutes(V, E, origin, destination, K)
# print the routes
for i in routes:
# the route as a list of strings, where each element corresponds to a link's name
route = i[0]
