Merge pull request #18 from ipa-fmw/master

config for cob3-6

cob_bringup/CMakeLists.txt

@@ -53,6 +53,11 @@ rosbuild_add_roslaunch_check(robots/cob3-4.xml ROBOT=cob3-4)
 rosbuild_add_roslaunch_check(tools ROBOT=cob3-4 ROBOT_ENV=ipa-kitchen)
 rosbuild_add_roslaunch_check(tools ROBOT=cob3-4 ROBOT_ENV=ipa-apartment)
 
+# rostest for cob3-6
+rosbuild_add_roslaunch_check(robots/cob3-6.xml ROBOT=cob3-6)
+rosbuild_add_roslaunch_check(tools ROBOT=cob3-6 ROBOT_ENV=ipa-kitchen)
+rosbuild_add_roslaunch_check(tools ROBOT=cob3-6 ROBOT_ENV=ipa-apartment)
+
 # rostest for raw3-1
 rosbuild_add_roslaunch_check(robots/raw3-1.xml ROBOT=raw3-1)
 rosbuild_add_roslaunch_check(tools ROBOT=raw3-1 ROBOT_ENV=ipa-kitchen)

cob_bringup/manifest.xml

@@ -42,6 +42,7 @@
   <depend package="cob_teleop"/>
   <depend package="cob_collision_velocity_filter"/>
   <depend package="cob_footprint_observer"/>
+  <depend package="cob_base_velocity_smoother"/>
 
   <depend package="joy"/>
   <depend package="rviz"/>

cob_bringup/robots/cob3-6.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+
+	<include file="$(find cob_bringup)/robots/cob3-6.xml" >
+		<arg name="pc1" value="cob3-6-pc1"/>
+		<arg name="pc2" value="cob3-6-pc2"/>
+		<arg name="pc3" value="cob3-3-pc3"/>
+	</include>
+
+</launch>

cob_bringup/robots/cob3-6.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<launch>
+
+	<arg name="pc1" default="localhost"/>
+	<arg name="pc2" default="localhost"/>
+	<arg name="pc3" default="localhost"/>
+
+	<group>
+		<machine name="$(arg pc1)" address="$(arg pc1)" default="true"/>
+
+		<!-- pc monitor -->
+		<include file="$(find cob_bringup)/tools/pc_monitor.launch" >
+			<arg name="pc" value="$(arg pc1)" />
+		</include>
+
+		<!-- upload robot description -->
+		<include file="$(find cob_hardware_config)/common/upload_robot.launch" />
+
+		<!-- start robot_state_publisher -->
+		<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher"/>
+
+		<!-- startup hardware -->
+		<!-- lower robot -->
+		<include file="$(find cob_bringup)/components/laser_front.launch" />
+		<include file="$(find cob_bringup)/components/laser_rear.launch" />
+		<include file="$(find cob_bringup)/components/laser_top.launch" />
+		<include file="$(find cob_bringup)/components/relayboard.launch" />
+		<include file="$(find cob_bringup)/components/base.launch" />
+
+		<!-- upper robot -->
+		<include file="$(find cob_bringup)/components/torso.launch" />
+		<include file="$(find cob_bringup)/components/tray.launch" />
+		<include file="$(find cob_bringup)/components/lwa.launch" />
+		<include file="$(find cob_bringup)/components/sdh.launch" />
+		<include file="$(find cob_bringup)/components/head.launch" />
+
+		<!-- start additional packages -->
+		<include file="$(find cob_bringup)/tools/diagnostics_aggregator.launch" />
+		<include file="$(find cob_bringup)/tools/teleop.launch" />
+		<include file="$(find cob_bringup)/tools/base_collision_observer.launch" />
+	</group>
+
+	<group>
+		<machine name="$(arg pc2)" address="$(arg pc2)" default="true"/>
+
+                <!-- pc monitor -->
+                <include file="$(find cob_bringup)/tools/pc_monitor.launch" >
+                        <arg name="pc" value="$(arg pc2)" />
+                </include>
+
+		<!-- start camera sensors -->
+		<include file="$(find cob_bringup)/components/prosilica_left.launch" />
+		<include file="$(find cob_bringup)/components/prosilica_right.launch" />
+		<include file="$(find cob_bringup)/components/kinect.launch" />
+	</group>
+
+	<machine name="$(arg pc1)" address="$(arg pc1)" default="true"/>
+
+</launch>

cob_controller_configuration_gazebo/CMakeLists.txt

@@ -21,6 +21,7 @@ rosbuild_add_roslaunch_check(ros/launch/default_controllers_cob3-1.launch ROBOT=
 rosbuild_add_roslaunch_check(ros/launch/default_controllers_cob3-2.launch ROBOT=cob3-2)
 rosbuild_add_roslaunch_check(ros/launch/default_controllers_cob3-3.launch ROBOT=cob3-3)
 rosbuild_add_roslaunch_check(ros/launch/default_controllers_cob3-4.launch ROBOT=cob3-4)
+rosbuild_add_roslaunch_check(ros/launch/default_controllers_cob3-6.launch ROBOT=cob3-6)
 
 # rostest raw
 rosbuild_add_roslaunch_check(ros/launch/default_controllers_raw3-1.launch ROBOT=raw3-1)

cob_controller_configuration_gazebo/ros/controller/arm_controller_cob3-6.yaml

@@ -0,0 +1,58 @@
+arm_controller:
+  type: robot_mechanism_controllers/JointSplineTrajectoryController
+  joints:
+    - arm_1_joint
+    - arm_2_joint
+    - arm_3_joint
+    - arm_4_joint
+    - arm_5_joint
+    - arm_6_joint
+    - arm_7_joint
+  gains:
+    arm_1_joint:
+      p: 2000.0
+      d: 30.0
+    arm_2_joint:
+      p: 2000.0
+      d: 30.0
+    arm_3_joint: 
+      p: 2000.0
+      d: 0.0
+    arm_4_joint:
+      p: 2020.0
+      d: 0.0
+    arm_5_joint:
+      p: 2030.0
+      d: 0.0
+    arm_6_joint:
+      p: 2050.0
+      d: 0.0
+    arm_7_joint:
+      p: 2080.0
+      d: 0.0
+
+  arm_joint_trajectory_action_node:
+    joints:
+      - arm_1_joint
+      - arm_2_joint
+      - arm_3_joint
+      - arm_4_joint
+      - arm_5_joint
+      - arm_6_joint
+      - arm_7_joint
+    constraints:
+      goal_time: 0.6
+      arm_1_joint:
+        goal: 0.1
+      arm_2_joint:
+        goal: 0.1
+      arm_3_joint:
+        goal: 0.1
+      arm_4_joint:
+        goal: 0.1
+      arm_5_joint:
+        goal: 0.1
+      arm_6_joint:
+        goal: 0.1
+      arm_7_joint:
+        goal: 0.1

cob_controller_configuration_gazebo/ros/launch/default_controllers_cob3-6.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+
+	<!-- load cob controllers -->
+	<include file="$(find cob_controller_configuration_gazebo)/ros/launch/default_controllers_cob.launch" />
+
+	<!-- Torso -->
+	<include file="$(find cob_controller_configuration_gazebo)/ros/launch/torso_v1_controller.launch"/>
+
+</launch>

cob_default_robot_config/CMakeLists.txt

@@ -41,6 +41,9 @@ rosbuild_add_roslaunch_check(upload_param.launch ROBOT=cob3-3)
 # rostest for cob3-4
 rosbuild_add_roslaunch_check(upload_param.launch ROBOT=cob3-4)
 
+# rostest for cob3-6
+rosbuild_add_roslaunch_check(upload_param.launch ROBOT=cob3-6)
+
 # rostest for raw3-1
 rosbuild_add_roslaunch_check(upload_param.launch ROBOT=raw3-1)
 

cob_default_robot_config/cob3-6/arm_joint_configurations.yaml

@@ -0,0 +1,18 @@
+joint_names: ["arm_1_joint","arm_2_joint","arm_3_joint","arm_4_joint","arm_5_joint","arm_6_joint","arm_7_joint"]
+
+# single positions
+home: [[0.0,0.0,0.0,0.0,0.0,0.0,0.0]]
+folded: [[2.4305, -2.0183, 2.3888, 1.7423, 0.1602, 0.9799, -1.4252]]
+pregrasp: [[1.7382, -1.4889, 0.3862, 1.9651, -3.4248, 1.7906, 0.2902]]
+grasp: [[1.5048, -1.6054, 0.7639, 1.2717, -3.1488, 1.1876, 0.0668]]
+overtray: [[-0.8402, -1.6393, 1.3151, 1.6565, 0.9701, 1.3971, -3.8298]]
+tray: [[-0.7411, -1.7354, 1.3658, 1.6142, 1.0109, 1.4441, -3.7285]]
+intermediateback: [[0.9491, -1.9267, 1.0229, 1.3607, 0.9578, 1.6990, -2.6809]]
+intermediatefront: [[-0.8894, -1.5624, 0.8592, 1.7863, 1.3653, 1.3339, -4.4183]]
+wavein: [[-0.1, 0.5, 0.0, 0.5, 0.0, 0.5, -1.5708]]
+waveout: [[-0.1, 0.5, 0.0, -0.5, 0.0, -0.5, -1.5708]]
+
+#trajectories
+grasp-to-tray: [pregrasp, intermediateback, intermediatefront, overtray]
+tray-to-folded: [intermediatefront, intermediateback, folded]
+wave: [waveout, wavein, waveout, wavein]

cob_default_robot_config/cob3-6/command_gui_buttons.yaml

@@ -0,0 +1,86 @@
+#group:
+#  group_name: <group_name>
+#  component_name: <component_name>
+#  buttons: [[button_name1,function_name1,parameter_name2,
+#    [button_name2,function_name2,parameter_name2]]
+
+group1:
+  group_name: base
+  component_name: base
+  buttons: [[stop,stop,omni],
+    [init,trigger,init],
+    [recover,trigger,recover]]
+group2:
+  group_name: torso
+  component_name: torso
+  buttons: [[stop,trigger,stop],
+    [init,trigger,init],
+    [recover,trigger,recover],
+    [enable joystick,mode,velocity],
+    [home,move,home],
+    [front,move,front],
+    [back,move,back],
+    [left,move,left],
+    [right,move,right],
+    [shake,move,shake],
+    [nod,move,nod]]
+group3:
+  group_name: tray
+  component_name: tray
+  buttons: [[stop,trigger,stop],
+    [init,trigger,init],
+    [recover,trigger,recover],
+    [enable joystick,mode,velocity],
+    [up,move,up],
+    [down,move,down]]
+group4:
+  group_name: arm settings
+  component_name: arm
+  buttons: [[stop,trigger,stop],
+    [init,trigger,init],
+    [recover,trigger,recover],
+    [undefined mode,mode,undefined],
+    [position mode,mode,position],
+    [velocity mode,mode,velocity],
+    [gravcomp mode,mode,gravcomp]]
+group5:
+  group_name: arm pos
+  component_name: arm
+  buttons: [[stop,trigger,stop],
+    [home,move,home],
+    [folded,move,folded],
+    [pregrasp,move,pregrasp],
+    [hold,move,hold],
+    [overtray,move,overtray],
+    [tray,move,tray],
+    [wave in,move,wavein],
+    [wave out,move,waveout]]
+group6:
+  group_name: arm traj
+  component_name: arm
+  buttons: [[stop,trigger,stop],
+    [folded-to-look at table,move,folded-to-look_at_table],
+    [look_at_table-to-folded,move,look_at_table-to-folded],
+    [grasp-to-tray,move,grasp-to-tray],
+    [tray-to-folded,move,tray-to-folded],
+    [wave,move,wave]]
+group7:
+  group_name: sdh
+  component_name: sdh
+  buttons: [[stop,trigger,stop],
+    [init,trigger,init],
+    [home,move,home],
+    [cyl. closed,move,cylclosed],
+    [cyl. open,move,cylopen],
+    [spher. closed,move,spherclosed],
+    [spher. open,move,spheropen],
+    [point,move,point],
+    [fist,move,fist]]
+group8:
+  group_name: eyes
+  component_name: head
+  buttons: [[stop,trigger,stop],
+    [init,trigger,init],
+    [recover,trigger,recover],
+    [front,move,front],
+    [back,move,back]]

cob_default_robot_config/cob3-6/head_joint_configurations.yaml

@@ -0,0 +1,3 @@
+joint_names: ["head_axis_joint"]
+front: [[-3.1415926]]
+back: [[0.0]]

cob_default_robot_config/cob3-6/light_configurations.yaml

@@ -0,0 +1,5 @@
+red: [999,0,0]
+green: [0,999,0]
+yellow: [400,999,0]
+blue: [0,0,999]
+white: [250,999,300]

cob_default_robot_config/cob3-6/robot_modules.yaml

@@ -0,0 +1 @@
+robot_modules: ['base','tray','torso','head','arm','sdh']

cob_default_robot_config/cob3-6/sdh_joint_configurations.yaml

@@ -0,0 +1,11 @@
+joint_names: ["sdh_knuckle_joint", "sdh_thumb_2_joint", "sdh_thumb_3_joint", "sdh_finger_12_joint", "sdh_finger_13_joint", "sdh_finger_22_joint", "sdh_finger_23_joint"]
+home: [[0.0,0.0,0.0,0.0,0.0,0.0,0.0]]
+cylclosed: [[0.0,0.0,1.0472,0.0,1.0472,0.0,1.0472]]
+cylopen: [[0.0,-0.9854,0.9472,-0.9854,0.9472,-0.9854,0.9472]]
+cyltotalopen: [[0.0,-1.57,-1.57,-1.57,-1.57,-1.57,-1.57]]
+spheropen: [[1.047,-0.785,1.047,-0.785,1.047,-0.785,1.047]]
+spherclosed: [[1.047,-0.262,1.047,-0.262,1.047,-0.262,1.047]]
+paropen: [[-0.524,0,0,1.57,0.524,-0.524,0.524]]
+parclosed: [[1.57,0,1.57,0,0,0,0]]
+point: [[0,0,1.57,0,1.57,0,0]]
+fist: [[0,0,1.57,0,1.57,0,1.57]]

cob_default_robot_config/cob3-6/sound_configurations.yaml

@@ -0,0 +1,41 @@
+speech_mode: FEST_EN
+#wav_de_path: /home/cob/svn/cob/Files/Audio/WiMi/wav_de/
+wav_en_path: /home/cob/svn/cob/Files/Audio/WiMi/wav_de/
+temp_wav_file: /home/fmw/wav/swift_temp.wav
+
+rand/introduce: ["sent01","sent02","sent03"]
+rand/offer: ["sent04","sent05","sent06","sent07","sent08","sent09"]
+rand/taken: ["sent10","sent11","sent12","sent13"]
+rand/pathblocked: ["sent14","sent15","sent16","sent17","sent18"]
+rand/load: ["sent21","sent22","sent23","sent24"]
+rand/enterRoom: ["sent25","sent26"]
+
+de/sent00: ["Hallo, ich bin Care-O-bot 3."]
+de/sent01: ["In dieser Woche biete ich hier Getränke an."]
+de/sent02: ["Ich habe die Aufgabe, Sie mit Getränken zu versorgen."]
+de/sent03: ["In dieser Woche unterstütze ich die Pflegekräfte bei Ihrer Arbeit."]
+de/sent04: ["Hallo. Darf ich jemandem etwas zu Trinken anbieten?"]
+de/sent05: ["Hallo. Möchte jemand einen Becher Wasser? Bitte greifen Sie zu!"]
+de/sent06: ["Hallo. Kann ich Ihnen etwas zu Trinken anbieten?"]
+de/sent07: ["Da bin ich wieder. Und ich habe etwas zu Trinken dabei. Sie können es einfach von meinem Tablett herunternehmen."]
+de/sent08: ["Ich habe hier einen Becher Wasser. Wenn Sie ihn haben möchten nehmen Sie ihn einfach von meinem Tablett."]
+de/sent09: ["Darf ich Ihnen etwas Wasser anbieten? Bitte greifen Sie zu!"]
+de/sent10: ["Danke, dass Sie mir etwas abgenommen haben."]
+de/sent11: ["Bittesehr."]
+de/sent12: ["Nett, dass Sie etwas genommen haben!"]
+de/sent13: ["Bitteschön!"]
+de/sent14: ["Entschuldingung. Dürfte ich mal vorbei?"]
+de/sent15: ["Entschuldigen Sie, ich muss hier durch!"]
+de/sent16: ["Könnten Sie wohl einen Schritt zur Seite treten? Ich muss hier durch!"]
+de/sent17: ["Hallo! Könnten Sie mich bitte vorbeilassen?"]
+de/sent18: ["Tut mir leid, Sie zu stören. Dürfte ich mal vorbei?"]
+de/sent19: ["Ich kann nicht losfahren. Mein Weg ist blockiert."]
+de/sent20: ["Ich geb's auf!"]
+de/sent21: ["Bitte stellen Sie ein Getränk auf mein Tablett."]
+de/sent22: ["Sellen Sie jetzt ein Getränk auf mein Tablett."]
+de/sent23: ["Ich kann das Getränk nun entgegennehmen."]
+de/sent24: ["Ich bin bereit."]
+de/sent25: ["Hallo."]
+de/sent26: ["Da bin ich wieder."]
+
+en/sent00: ["Hello, I am Care-O-Bot 3."]

cob_default_robot_config/cob3-6/torso_joint_configurations.yaml

@@ -0,0 +1,17 @@
+joint_names: ["torso_lower_neck_tilt_joint","torso_pan_joint","torso_upper_neck_tilt_joint"]
+home: [[0.0,0.0,0.0]]
+left: [[0.0,-0.1,0.0]]
+right: [[0.0,0.1,0.0]]
+front: [[-0.1,0.0,-0.15]]
+front_left: [[-0.1,-0.1,-0.15]]
+front_right: [[-0.1,0.1,-0.15]]
+back: [[0.1,0.0,0.15]]
+back_extreme: [[0.20,0.0,0.35]]
+back_left: [[0.1,-0.1,0.15]]
+back_left_extreme: [[0.20,-0.1,0.35]]
+back_right: [[0.1,0.1,0.15]]
+back_right_extreme: [[0.20,0.1,0.35]]
+nod: [front,home]
+lookontray: [[-0.174,0,-0.262]]
+bow: [[-0.15,0,-0.3], [-0.15,0,-0.3], home]
+shake: [left, right, left, home]

cob_default_robot_config/cob3-6/tray_joint_configurations.yaml

@@ -0,0 +1,3 @@
+joint_names: ["tray_joint"]
+up: [[0.0]]
+down: [[-3.1415926]]

cob_default_robot_config/cob3-6/upload_param_cob3-6.launch

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+
+	<!-- send ROBOT parameters to parameter server -->
+	<rosparam command="load" ns="/script_server/tray" file="$(find cob_default_robot_config)/$(env ROBOT)/tray_joint_configurations.yaml"/>
+	<rosparam command="load" ns="/script_server/torso" file="$(find cob_default_robot_config)/$(env ROBOT)/torso_joint_configurations.yaml"/>
+	<rosparam command="load" ns="/script_server/head" file="$(find cob_default_robot_config)/$(env ROBOT)/head_joint_configurations.yaml"/>
+	<rosparam command="load" ns="/script_server/arm" file="$(find cob_default_robot_config)/$(env ROBOT)/arm_joint_configurations.yaml"/>
+	<rosparam command="load" ns="/script_server/sdh" file="$(find cob_default_robot_config)/$(env ROBOT)/sdh_joint_configurations.yaml"/>
+	<rosparam command="load" ns="/script_server/light" file="$(find cob_default_robot_config)/$(env ROBOT)/light_configurations.yaml"/>
+	<rosparam command="load" ns="/script_server/sound" file="$(find cob_default_robot_config)/$(env ROBOT)/sound_configurations.yaml"/>
+
+</launch>