-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdemo copy.py
executable file
·166 lines (123 loc) · 3.59 KB
/
demo copy.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
159
160
161
162
163
164
165
#!/usr/bin/env python
# import system module
# import sys
# import os
# sys.path.append("~/my_pkg")
# import subprocess
import iwb_ros.robot
import iwb_ros.test
# # start roscore
# print("launch roscore: start")
# try:
# subprocess.Popen(['roscore'], stdout=subprocess.PIPE)
# print("launch roscore: ok")
# except:
# print("start roscore: error")
# instantiation
print("creat robot object: start")
# robot = iwb_ros.robot.IWB_Robot()
try:
robot = iwb_ros.robot.IWB_Robot()
print("creat robot object: ok")
except:
print("!!!Fatal: create obj failed.!!!")
# #################################### visualization code ################################
# print("launch visualization: start")
# # start rviz
# robot.visualization()
# # start fake controller
# robot.fake_controller()
# # user self-programmed code with set function
# # a simple example:
# import rosnode
# print(rosnode.get_node_names())
# joint_position = [0]*18
# i = 0
# n = 0.01
# t = 0
# while True:
# t = t + 1
# # print(joint_position)
# joint_position[0] = i
# joint_position[1] = i
# joint_position[2] = i
# joint_position[17] = i
# joint_position[14] = i
# robot.send_joint_position(joint_position)
# i = i + n
# if i > 3.14:
# i = -3.14
# if t >= 2000:
# break
# robot.shutdown_all()
# ##################################################################################
################################# test block ###################################
print("launch motion planning: start")
# start rviz
robot.visualization()
from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
# test roslib
# import roslib
# roslib.load_manifest("pykdl_utils")
# problematisch import
import pykdl_utils.joint_kinematics
from pykdl_utils.kdl_kinematics import KDLKinematics
kdl_robot = URDF.from_parameter_server()
tree = kdl_tree_from_urdf_model(kdl_robot)
print(tree.getNrOfSegments())
base_link = "base_link"
end_link = "link_6_x"
chain = tree.getChain(base_link, end_link)
test_kdl_obj = KDLKinematics(kdl_robot, base_link, end_link)
print(test_kdl_obj.get_joint_names())
print(chain.getNrOfJoints())
# q = test_kdl_obj.random_joint_angles()
q = [0]*18
jcb = test_kdl_obj.jacobian(q)
mass = test_kdl_obj.inertia(q)
mass_cart = test_kdl_obj.cart_inertia(q)
import numpy as np
with open('outfile.txt', 'wb') as f:
for line in mass:
np.savetxt(f, line, fmt='%.2f')
print(mass)
print("J")
print(jcb)
################################################################################
#################################### motion planning code ##########################
# print("launch motion planning: start")
# # start rviz
# robot.motion_visualization()
# from urdf_parser_py.urdf import URDF
# from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
# kdl_robot = URDF.from_parameter_server()
# tree = kdl_tree_from_urdf_model(kdl_robot)
# print(tree.getNrOfSegments())
# chain = tree.getChain(base_link, end_link)
# print(chain.getNrOfJoints())
# # start fake controller
# robot.fake_controller()
# # user self-programmed code with set function
# # a simple example:
# import rosnode
# print(rosnode.get_node_names())
# joint_position = [0]*18
# i = 0
# n = 0.01
# t = 0
# while True:
# t = t + 1
# # print(joint_position)
# joint_position[0] = i
# joint_position[1] = i
# joint_position[2] = i
# joint_position[17] = i
# joint_position[14] = i
# robot.send_joint_position(joint_position)
# i = i + n
# if i > 3.14:
# i = -3.14
# if t >= 2000:
# break
robot.shutdown_all()