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(): ## @ロボットをエスケープ姿勢まで動かす
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 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)