-
Notifications
You must be signed in to change notification settings - Fork 0
/
client_abb.py
66 lines (49 loc) · 2.15 KB
/
client_abb.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
from RobotRaconteur.Client import *
import yaml, time, traceback, threading, sys
import numpy as np
sys.path.append('toolbox')
from vel_emulate_sub import EmulatedVelocityControl
###connect to all RR services
planner_inst=RRN.ConnectService('rr+tcp://localhost:25522/?service=Planner')
ABB_sub=RRN.SubscribeService('rr+tcp://localhost:58655?service=robot')
state_w = ABB_sub.SubscribeWire("robot_state")
cmd_w=ABB_sub.SubscribeWire('position_command')
ABB=ABB_sub.GetDefaultClientWait(1)
vel_ctrl = EmulatedVelocityControl(ABB,state_w, cmd_w)
###RR constants
robot_const = RRN.GetConstants("com.robotraconteur.robotics.robot", ABB)
halt_mode = robot_const["RobotCommandMode"]["halt"]
jog_mode = robot_const["RobotCommandMode"]["jog"]
trajectory_mode = robot_const["RobotCommandMode"]["trajectory"]
position_mode = robot_const["RobotCommandMode"]["position_command"]
velocity_mode = robot_const["RobotCommandMode"]["velocity_command"]
###Command mode
ABB.command_mode = halt_mode
time.sleep(0.1)
ABB.command_mode = jog_mode
start_joint_ABB=np.array([0.64833199, 0.99963736,-0.99677272,-0. , 1.56793168, 0.64833199])
ABB.jog_freespace(start_joint_ABB,np.ones(6),True)
planner_inst.state_prop('abb',np.zeros(6*planner_inst.N_step)) ###update position
###Command mode
ABB.command_mode = halt_mode
time.sleep(0.1)
ABB.command_mode = position_mode
vel_ctrl.enable_velocity_mode()
qd=np.array([-0.64833199, 0.99963736,-0.99677272,-0. , 1.56793168, 0.64833199])
planner_inst.plan_initial('abb',qd,30)
while np.linalg.norm(state_w.InValue.joint_position-qd)>0.1:
qdot=planner_inst.plan('abb',qd)
print(qdot)
now=time.time()
while time.time()-now<planner_inst.ts:
vel_ctrl.set_velocity_command(qdot)
vel_ctrl.set_velocity_command(np.zeros((6,)))
planner_inst.plan_initial('abb',start_joint_ABB,30)
while np.linalg.norm(state_w.InValue.joint_position-start_joint_ABB)>0.1:
qdot=planner_inst.plan('abb',start_joint_ABB)
print(qdot)
now=time.time()
while time.time()-now<planner_inst.ts:
vel_ctrl.set_velocity_command(qdot)
vel_ctrl.set_velocity_command(np.zeros((6,)))
vel_ctrl.disable_velocity_mode()