-
Notifications
You must be signed in to change notification settings - Fork 0
/
tes_env.py
169 lines (121 loc) · 6.78 KB
/
tes_env.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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
#!/usr/bin/env python3
from tesseract_robotics.tesseract_environment import Environment, ChangeJointOriginCommand, MoveJointCommand
from tesseract_robotics import tesseract_geometry
from tesseract_robotics.tesseract_common import Isometry3d, CollisionMarginData, Translation3d, Quaterniond, \
ManipulatorInfo
from tesseract_robotics import tesseract_collision
from tesseract_robotics_viewer import TesseractViewer
import RobotRaconteur as RR
RRN=RR.RobotRaconteurNode.s
import yaml, time, traceback, threading, sys, json
import numpy as np
from scipy.optimize import fminbound
from gazebo_model_resource_locator import GazeboModelResourceLocator
from robots_def import *
#convert 4x4 H matrix to 3x3 H matrix and inverse for mapping obj to robot frame
def H42H3(H):
H3=np.linalg.inv(H[:2,:2])
H3=np.hstack((H3,-np.dot(H3,np.array([[H[0][-1]],[H[1][-1]]]))))
H3=np.vstack((H3,np.array([0,0,1])))
return H3
class Tess_Env(object):
def __init__(self,urdf_path,robot_linkname,robot_jointname):
self.robot_linkname=robot_linkname
self.robot_jointname=robot_jointname
######tesseract environment setup:
with open(urdf_path+'.urdf','r') as f:
combined_urdf = f.read()
with open(urdf_path+'.srdf','r') as f:
combined_srdf = f.read()
self.t_env= Environment()
self.t_env.init(combined_urdf, combined_srdf, GazeboModelResourceLocator())
self.scene_graph=self.t_env.getSceneGraph()
#Tesseract reports all GJK/EPA distance within contact_distance threshold
contact_distance=0.1
monitored_link_names = self.t_env.getLinkNames()
self.manager = self.t_env.getDiscreteContactManager()
self.manager.setActiveCollisionObjects(monitored_link_names)
self.manager.setCollisionMarginData(CollisionMarginData(contact_distance))
#######viewer setup, for URDF setup verification in browser @ localhost:8000/#########################
self.viewer = TesseractViewer()
self.viewer.update_environment(self.t_env, [0,0,0])
self.viewer.start_serve_background()
def update_pose(self,model_name,H):
###update model pose in tesseract environment
cmd = ChangeJointOriginCommand(model_name+'_pose', Isometry3d(H))
self.t_env.applyCommand(cmd)
#refresh
self.viewer.update_environment(self.t_env)
def attach_part(self,model_name,link_name,H=np.eye(4)):
cmd = MoveJointCommand(model_name+'_pose', link_name)
self.t_env.applyCommand(cmd)
cmd = ChangeJointOriginCommand(model_name+'_pose', H)
self.t_env.applyCommand(cmd)
#refresh
self.viewer.update_environment(self.t_env)
def check_collision_single(self,robot_name,q):
###check collision for a single robot, including self collision and collision with part
self.t_env.setState(self.robot_jointname[robot_name], q)
env_state = self.t_env.getState()
self.manager.setCollisionObjectsTransform(env_state.link_transforms)
result = tesseract_collision.ContactResultMap()
contacts = self.manager.contactTest(result,tesseract_collision.ContactRequest(tesseract_collision.ContactTestType_ALL))
result_vector = tesseract_collision.ContactResultVector()
result.flattenMoveResults(result_vector)
###iterate all collision instances
for c in result_vector:
if (c.link_names[0] in self.robot_linkname[robot_name] or c.link_names[1] in self.robot_linkname[robot_name]):
print(c.link_names[0], c.link_names[1], c.distance)
return True
return False
#######################################update joint angles in Tesseract Viewer###########################################
def viewer_joints_update(self,robot_name,joints):
self.viewer.update_joint_positions(self.robot_jointname[robot_name], np.array(joints))
def viewer_trajectory(self,robot_name,curve_js):
trajectory_json = dict()
trajectory_json["use_time"] = True
trajectory_json["loop_time"] = 20
trajectory_json["joint_names"] = self.robot_jointname[robot_name]
trajectory2 = np.hstack((curve_js,np.linspace(0,10,num=len(curve_js))[np.newaxis].T))
trajectory_json["trajectory"] = trajectory2.tolist()
self.viewer.trajectory_json=json.dumps(trajectory_json)
def main_abb():
#link and joint names in urdf
ABB_6640_180_255_joint_names=["ABB_6640_180_255_joint_1","ABB_6640_180_255_joint_2","ABB_6640_180_255_joint_3","ABB_6640_180_255_joint_4","ABB_6640_180_255_joint_5","ABB_6640_180_255_joint_6"]
ABB_6640_180_255_link_names=["ABB_6640_180_255_link_1","ABB_6640_180_255_link_2","ABB_6640_180_255_link_3","ABB_6640_180_255_link_4","ABB_6640_180_255_link_5","ABB_6640_180_255_link_6"]
#Robot dictionaries, all reference by name
robot_linkname={'ABB_6640_180_255':ABB_6640_180_255_link_names}
robot_jointname={'ABB_6640_180_255':ABB_6640_180_255_joint_names}
t=Tess_Env('config/urdf/abb_cell',robot_linkname,robot_jointname) #create obj
input("Press enter to quit")
#stop background checker
def collision_test_abb():
#link and joint names in urdf
ABB_6640_180_255_joint_names=["ABB_6640_180_255_joint_1","ABB_6640_180_255_joint_2","ABB_6640_180_255_joint_3","ABB_6640_180_255_joint_4","ABB_6640_180_255_joint_5","ABB_6640_180_255_joint_6"]
ABB_6640_180_255_link_names=["ABB_6640_180_255_link_1","ABB_6640_180_255_link_2","ABB_6640_180_255_link_3","ABB_6640_180_255_link_4","ABB_6640_180_255_link_5","ABB_6640_180_255_link_6"]
#Robot dictionaries, all reference by name
robot_linkname={'ABB_6640_180_255':ABB_6640_180_255_link_names}
robot_jointname={'ABB_6640_180_255':ABB_6640_180_255_joint_names}
t=Tess_Env('config/urdf/abb_cell',robot_linkname,robot_jointname) #create obj
q=np.array([1.1,0.5,0.7,1,1,1.])
t.viewer_joints_update('ABB_6640_180_255',q)
print(t.check_collision_single('ABB_6640_180_255',q))
input("Press enter to quit")
def main_motoman():
#link and joint names in urdf
MA2010_link_names=["MA2010_base_link","MA2010_link_1_s","MA2010_link_2_l","MA2010_link_3_u","MA2010_link_4_r","MA2010_link_5_b","MA2010_link_6_t"]
MA2010_joint_names=["MA2010_joint_1_s","MA2010_joint_2_l","MA2010_joint_3_u","MA2010_joint_4_r","MA2010_joint_5_b","MA2010_joint_6_t"]
MA1440_link_names=["MA1440_base_link","MA1440_link_1_s","MA1440_link_2_l","MA1440_link_3_u","MA1440_link_4_r","MA1440_link_5_b","MA1440_link_6_t"]
MA1440_joint_names=["MA1440_joint_1_s","MA1440_joint_2_l","MA1440_joint_3_u","MA1440_joint_4_r","MA1440_joint_5_b","MA1440_joint_6_t"]
D500B_joint_names=["D500B_joint_1","D500B_joint_2"]
D500B_link_names=["D500B_base_link","D500B_link_1","D500B_link_2"]
#Robot dictionaries, all reference by name
robot_linkname={'MA2010_A0':MA2010_link_names,'MA1440_A0':MA1440_link_names,'D500B':D500B_link_names}
robot_jointname={'MA2010_A0':MA2010_joint_names,'MA1440_A0':MA1440_joint_names,'D500B':D500B_joint_names}
t=Tess_Env('config/urdf/motoman_cell',robot_linkname,robot_jointname) #create obj
# q=np.array([1.1,0.5,0.7,1,1,1.])
q=np.array([0,0,1.57,0,0,0])
t.viewer_joints_update('MA2010_A0',q)
input("Press enter to quit")
if __name__ == '__main__':
main_motoman()