Skip to content

Commit d286148

Browse files
lentinlentin
committed
Updated first code commit
1 parent c8a2585 commit d286148

File tree

643 files changed

+112429
-0
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

643 files changed

+112429
-0
lines changed

‎chapter_10_codes/cool_arm/cool5000_controller/CMakeLists.txt

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(cool5000_controller)
3+
4+
## Find catkin macros and libraries
5+
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
6+
## is used, also find other catkin packages
7+
8+
# Load catkin and all dependencies required for this package
9+
find_package(catkin REQUIRED COMPONENTS
10+
roscpp
11+
rospy
12+
std_msgs
13+
actionlib
14+
dynamixel_msgs
15+
sensor_msgs
16+
tf
17+
)
18+
19+
# Declare a catkin package
20+
catkin_package(
21+
CATKIN_DEPENDS
22+
roscpp
23+
rospy
24+
std_msgs
25+
actionlib
26+
std_msgs
27+
sensor_msgs
28+
dynamixel_msgs
29+
tf
30+
)
31+
32+
## Build
33+
include_directories(
34+
include
35+
${catkin_INCLUDE_DIRS}
36+
)
37+
38+
# ---------------
39+
add_executable(joint_state_aggregator src/joint_state_aggregator.cpp)
40+
target_link_libraries(joint_state_aggregator ${catkin_LIBRARIES} )

‎chapter_10_codes/cool_arm/cool5000_controller/config/cool5000.yaml

+100
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
joint1_controller:
2+
controller:
3+
package: dynamixel_controllers
4+
module: joint_position_controller
5+
type: JointPositionController
6+
joint_name: joint1
7+
joint_speed: 0.1
8+
motor:
9+
id: 0
10+
init: 2048
11+
min: 320
12+
max: 3823
13+
14+
15+
joint2_controller:
16+
controller:
17+
package: dynamixel_controllers
18+
module: joint_position_controller
19+
type: JointPositionController
20+
joint_name: joint2
21+
joint_speed: 0.1
22+
motor:
23+
id: 1
24+
init: 2048
25+
min: 957
26+
max: 3106
27+
28+
29+
joint3_controller:
30+
controller:
31+
package: dynamixel_controllers
32+
module: joint_position_controller
33+
type: JointPositionController
34+
joint_name: joint3
35+
joint_speed: 0.1
36+
motor:
37+
id: 2
38+
init: 2048
39+
min: 314
40+
max: 3836
41+
42+
43+
joint4_controller:
44+
controller:
45+
package: dynamixel_controllers
46+
module: joint_position_controller
47+
type: JointPositionController
48+
joint_name: joint4
49+
joint_speed: 0.1
50+
motor:
51+
id: 3
52+
init: 2048
53+
min: 750
54+
max: 3344
55+
56+
57+
joint5_controller:
58+
controller:
59+
package: dynamixel_controllers
60+
module: joint_position_controller
61+
type: JointPositionController
62+
joint_name: joint5
63+
joint_speed: 0.1
64+
motor:
65+
id: 4
66+
init: 2048
67+
min: 780
68+
max: 3270
69+
70+
71+
joint6_controller:
72+
controller:
73+
package: dynamixel_controllers
74+
module: joint_position_controller
75+
type: JointPositionController
76+
joint_name: joint6
77+
joint_speed: 0.1
78+
motor:
79+
id: 5
80+
init: 2048
81+
min: 780
82+
max: 3270
83+
84+
joint7_controller:
85+
controller:
86+
package: dynamixel_controllers
87+
module: joint_position_controller
88+
type: JointPositionController
89+
joint_name: joint7
90+
joint_speed: 0.1
91+
motor:
92+
id: 6
93+
init: 2048
94+
min: 356
95+
max: 3749
96+
97+
98+
99+
100+

‎chapter_10_codes/cool_arm/cool5000_controller/config/cool5000_trajectory_controller.yaml

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
cool5000_trajectory_controller:
2+
controller:
3+
package: dynamixel_controllers
4+
module: joint_trajectory_action_controller
5+
type: JointTrajectoryActionController
6+
joint_trajectory_action_node:
7+
min_velocity: 0.0
8+
constraints:
9+
goal_time: 0.01
10+
11+
12+
13+
14+
15+
16+
17+

‎chapter_10_codes/cool_arm/cool5000_controller/config/dynamixel_ports.yaml

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
namespace: cool5000_controller_manager
2+
diagnostics_rate: 5
3+
serial_ports:
4+
ttyUSB0:
5+
port_name: /dev/ttyUSB0
6+
baud_rate: 1000000
7+
min_motor_id: 0
8+
max_motor_id: 6
9+
update_rate: 50
10+
diagnostics:
11+
error_level_temp: 70
12+
warn_level_temp: 65
13+

‎chapter_10_codes/cool_arm/cool5000_controller/launch/cool5000_controller.launch

+54
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
<!-- -*- mode: XML -*- -->
2+
3+
<launch>
4+
5+
<!-- Start the Dynamixel motor manager to control all cool5000 servos -->
6+
<node name="dynamixel_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
7+
<rosparam>
8+
namespace: dxl_manager
9+
serial_ports:
10+
dynamixel_port:
11+
port_name: "/dev/ttyUSB0"
12+
baud_rate: 1000000
13+
min_motor_id: 0
14+
max_motor_id: 6
15+
update_rate: 20
16+
</rosparam>
17+
</node>
18+
19+
<!-- Load joint controller configuration from YAML file to parameter server -->
20+
<rosparam file="$(find cool5000_controller)/config/cool5000.yaml" command="load"/>
21+
22+
<!-- Start all SmartArm joint controllers -->
23+
<node name="controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
24+
args="--manager=dxl_manager
25+
--port dynamixel_port
26+
joint1_controller
27+
joint2_controller
28+
joint3_controller
29+
joint4_controller
30+
joint5_controller
31+
joint6_controller
32+
joint7_controller"
33+
output="screen"/>
34+
35+
<!-- Start the cool5000 arm trajectory controller -->
36+
<rosparam file="$(find cool5000_controller)/config/cool5000_trajectory_controller.yaml" command="load"/>
37+
<node name="controller_spawner_meta" pkg="dynamixel_controllers" type="controller_spawner.py"
38+
args="--manager=dxl_manager
39+
--type=meta
40+
cool5000_trajectory_controller
41+
joint1_controller
42+
joint2_controller
43+
joint3_controller
44+
joint4_controller
45+
joint5_controller
46+
joint6_controller
47+
joint7_controller"
48+
output="screen"/>
49+
50+
<!-- Publish combined joint info -->
51+
<include file="$(find cool5000_controller)/launch/joint_state_aggregator.launch" />
52+
53+
</launch>
54+

‎chapter_10_codes/cool_arm/cool5000_controller/launch/joint_state_aggregator.launch

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
<!-- -*- mode: XML -*- -->
2+
3+
<launch>
4+
<node name="joint_state_aggregator" pkg="cool5000_controller" type="joint_state_aggregator" output="screen">
5+
<rosparam>
6+
rate: 50
7+
controllers:
8+
- joint1_controller
9+
- joint2_controller
10+
- joint3_controller
11+
- joint4_controller
12+
- joint5_controller
13+
- joint6_controller
14+
- joint7_controller
15+
</rosparam>
16+
</node>
17+
</launch>

‎chapter_10_codes/cool_arm/cool5000_controller/package.xml

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
<?xml version="1.0"?>
2+
<package>
3+
<name>cool5000_controller</name>
4+
<version>0.0.0</version>
5+
<description>The cool5000_controller package</description>
6+
7+
<!-- One maintainer tag required, multiple allowed, one person per tag -->
8+
<!-- Example: -->
9+
<maintainer email="[email protected]">Ravi</maintainer>
10+
11+
12+
<!-- One license tag required, multiple allowed, one license per tag -->
13+
<!-- Commonly used license strings: -->
14+
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
15+
<license>BSD</license>
16+
17+
<author email="[email protected]">Ravi</author>
18+
19+
<build_depend>roscpp</build_depend>
20+
<build_depend>rospy</build_depend>
21+
<build_depend>actionlib</build_depend>
22+
<build_depend>actionlib_msgs</build_depend>
23+
<build_depend>dynamixel_controllers</build_depend>
24+
<build_depend>dynamixel_msgs</build_depend>
25+
<build_depend>std_msgs</build_depend>
26+
<build_depend>sensor_msgs</build_depend>
27+
<build_depend>wxpython</build_depend>
28+
<build_depend>tf</build_depend>
29+
30+
<run_depend>roscpp</run_depend>
31+
<run_depend>rospy</run_depend>
32+
<run_depend>actionlib</run_depend>
33+
<run_depend>actionlib_msgs</run_depend>
34+
<run_depend>dynamixel_controllers</run_depend>
35+
<run_depend>dynamixel_msgs</run_depend>
36+
<run_depend>std_msgs</run_depend>
37+
<run_depend>sensor_msgs</run_depend>
38+
<run_depend>wxpython</run_depend>
39+
<run_depend>tf</run_depend>
40+
41+
<buildtool_depend>catkin</buildtool_depend>
42+
43+
</package>

‎chapter_10_codes/cool_arm/cool5000_controller/scripts/cool5000_play.py

+66
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
#!/usr/bin/env python
2+
3+
import roslib; roslib.load_manifest('cool5000_controller')
4+
import time
5+
import rospy
6+
import pickle
7+
import traceback
8+
import sys
9+
from std_msgs.msg import Float64
10+
11+
joint_names = ('joint1',
12+
'joint2',
13+
'joint3',
14+
'joint4',
15+
'joint5',
16+
'joint6',
17+
'joint7')
18+
19+
20+
controller_names = ('joint1_controller',
21+
'joint2_controller',
22+
'joint3_controller',
23+
'joint4_controller',
24+
'joint5_controller',
25+
'joint6_controller',
26+
'joint7_controller')
27+
28+
def main():
29+
30+
pubs = [rospy.Publisher(name + '/command', Float64) for name in controller_names]
31+
rospy.init_node('motion_play', anonymous=True)
32+
33+
# determine file name to open
34+
if len(sys.argv) != 2:
35+
filename = 'dump.dat'
36+
else:
37+
filename = sys.argv[1]
38+
39+
f = open(filename,'r')
40+
read_coords = pickle.load(f)
41+
42+
print 'Playing...'
43+
44+
for coor in read_coords:
45+
print 'sending command', coor[1], "to", coor[0], "via", joint_names.index(coor[0])
46+
pubs[ joint_names.index(coor[0]) ].publish(coor[1])
47+
48+
time.sleep(.5)
49+
50+
print 'COMPLETE ---------------------------- '
51+
52+
53+
# Where the program starts
54+
if __name__ == "__main__":
55+
try:
56+
main()
57+
# Now attempt to prevent the robot from falling limp
58+
except KeyboardInterrupt:
59+
print "done"
60+
except Exception as e:
61+
traceback.print_exc(e)
62+
print "done"
63+
64+
65+
66+

0 commit comments

Comments
 (0)