Merge branch 'master' of ../rtmros_tutorials_filtered2 into add_sampl…

…e_test

hrpsys_ros_bridge/euslisp/samplerobot-interface.l

@@ -0,0 +1,67 @@
+(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l")
+(require :samplerobot "package://hrpsys_ros_bridge_tutorials/models/samplerobot.l")
+
+(defclass samplerobot-interface
+  :super rtm-ros-robot-interface
+  :slots ())
+(defmethod samplerobot-interface
+  (:init (&rest args)
+    (send-super* :init :robot samplerobot-robot args)
+    (mapcar #'(lambda (ctype)
+                (send self :add-controller ctype))
+            (send self :default-controller-list))
+    )
+  (:default-controller-list ()
+   (list :larm-controller
+         :rarm-controller
+         :torso-controller
+         :lhand-controller
+         :rhand-controller
+         :fullbody-controller))
+  (:default-controller ()
+    (mapcar #'(lambda (ctype) (car (send self ctype))) (send self :default-controller-list)))
+  (:fullbody-controller ()
+   (send-message self robot-interface :default-controller))
+  (:larm-controller ()
+   (list
+    (list
+     (cons :controller-action "/larm_controller/follow_joint_trajectory_action")
+     (cons :controller-state  "/larm_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (cons :joint-names (list "LARM_SHOULDER_P" "LARM_SHOULDER_R" "LARM_SHOULDER_Y" "LARM_ELBOW" "LARM_WRIST_Y" "LARM_WRIST_P")))))
+  (:rarm-controller ()
+   (list
+    (list
+     (cons :controller-action "/rarm_controller/follow_joint_trajectory_action")
+     (cons :controller-state  "/rarm_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (cons :joint-names (list "RARM_SHOULDER_P" "RARM_SHOULDER_R" "RARM_SHOULDER_Y" "RARM_ELBOW" "RARM_WRIST_Y" "RARM_WRIST_P")))))
+  (:torso-controller ()
+   (list
+    (list
+     (cons :controller-action "/torso_controller/follow_joint_trajectory_action")
+     (cons :controller-state  "/torso_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (cons :joint-names (list "WAIST_P" "WAIST_R" "CHEST")))))
+  (:lhand-controller ()
+   (list
+    (list
+     (cons :controller-action "/lhand_controller/follow_joint_trajectory_action")
+     (cons :controller-state  "/lhand_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (cons :joint-names (list "LARM_WRIST_R")))))
+  (:rhand-controller ()
+   (list
+    (list
+     (cons :controller-action "/rhand_controller/follow_joint_trajectory_action")
+     (cons :controller-state  "/rhand_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (cons :joint-names (list "RARM_WRIST_R")))))
+  )
+
+(defun samplerobot-init (&rest args)
+  (if (not (boundp '*ri*))
+      (setq *ri* (instance* samplerobot-interface :init args)))
+  (if (not (boundp '*sr*))
+      (setq *sr* (instance samplerobot-robot :init)))
+  )

hrpsys_ros_bridge/launch/samplerobot-grxui.launch

@@ -0,0 +1,35 @@
+<launch>
+  <arg name="KILL_SERVERS" default="false" />
+  <include file="$(find hrpsys_ros_bridge_tutorials)/launch/samplerobot_startup-grxui.launch" >
+    <arg name="KILL_SERVERS" default="$(arg KILL_SERVERS)" />
+  </include>
+  <include file="$(find hrpsys_ros_bridge_tutorials)/launch/samplerobot_ros_bridge-grxui.launch" />
+
+  <sphinxdoc><![CDATA[
+
+.. code-block:: bash
+
+  rosrun roseus roseus `rospack find hrpsys_ros_bridge_tutorials`/euslisp/samplerobot-pickup.l
+
+This launch file shows an example of ros bridge for sample robot and client program `samplerobot-pickup.l`
+
+.. video:: build/images/SampleRobot_PickUp
+  :width: 600
+
+.. video:: build/images/samplerobot_rviz-1
+  :width: 600
+
+  ]]></sphinxdoc>
+  <test type="test-grxui.py" pkg="openhrp3" time-limit="1200"
+   test-name="SampleRobot_PickUp"
+   args="--max-time=30
+	 --viewer-name='' #
+	 --no-start-simulation
+	 --target-directory=$(find hrpsys_ros_bridge_tutorials)/build/images
+	 --check-tf='/WAIST_LINK0 /VISION_SENSOR1'
+	 --script='rosrun roseus roseus $(find hrpsys_ros_bridge_tutorials)/euslisp/samplerobot-pickup.l __log:=dummy'" /> <!-- add dummy __log/= argument to invoke roseus with non-interactive mode -->
+  <test type="ogv_encode.sh" pkg="jsk_tools" test-name="z_SampleRobot_PickUp" args="$(find hrpsys_ros_bridge_tutorials)/build/images/SampleRobot_PickUp.ogv" time-limit="1000"/>
+  <test type="glc_encode.sh" pkg="jsk_tools" test-name="z_samplerobot_rviz" args="$(find hrpsys_ros_bridge_tutorials)/build/images/samplerobot_rviz.glc" time-limit="2000"/>
+</launch>
+
+

hrpsys_ros_bridge/launch/samplerobot.vcg

@@ -0,0 +1,163 @@
+Background\ ColorR=0
+Background\ ColorG=0
+Background\ ColorB=0
+Fixed\ Frame=/odom
+Target\ Frame=/odom
+Camera.Alpha=1
+Camera.Enabled=1
+Camera.Image\ Topic=/image_raw
+Camera.Transport\ Hint=raw
+Grid.Alpha=0.5
+Grid.Cell\ Size=1
+Grid.ColorR=0.5
+Grid.ColorG=0.5
+Grid.ColorB=0.5
+Grid.Enabled=1
+Grid.Line\ Style=0
+Grid.Line\ Width=0.03
+Grid.Normal\ Cell\ Count=0
+Grid.OffsetX=0
+Grid.OffsetY=0
+Grid.OffsetZ=0
+Grid.Plane=0
+Grid.Plane\ Cell\ Count=10
+Grid.Reference\ Frame=<Fixed Frame>
+Robot\ Model.Alpha=1
+Robot\ Model.Collision\ Enabled=0
+Robot\ Model.Enabled=1
+Robot\ Model.Robot\ Description=robot_description
+Robot\ Model.TF\ Prefix=
+Robot\ Model.Update\ Interval=0.5
+Robot\ Model.Visual\ Enabled=1
+Robot\:\ Robot\ Model\ Link\ LARM_LINK1Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK1Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK2Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK2Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK3Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK3Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK4Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK4Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK5Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK5Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK6Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK6Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK7Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LARM_LINK7Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK1Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK1Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK2Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK2Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK3Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK3Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK4Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK4Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK5Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK5Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK6Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ LLEG_LINK6Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK1Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK1Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK2Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK2Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK3Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK3Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK4Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK4Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK5Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK5Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK6Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK6Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK7Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RARM_LINK7Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK1Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK1Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK2Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK2Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK3Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK3Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK4Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK4Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK5Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK5Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK6Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ RLEG_LINK6Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ WAIST_LINK0Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ WAIST_LINK0Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ WAIST_LINK1Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ WAIST_LINK1Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ WAIST_LINK2Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ WAIST_LINK2Show\ Trail=0
+Robot\:\ Robot\ Model\ Link\ WAIST_LINK3Show\ Axes=0
+Robot\:\ Robot\ Model\ Link\ WAIST_LINK3Show\ Trail=0
+TF.All\ Enabled=1
+TF.Enabled=1
+TF.Frame\ Timeout=15
+TF.Show\ Arrows=1
+TF.Show\ Axes=1
+TF.Show\ Names=1
+TF.Update\ Interval=0.5
+Tool\ 2D\ Nav\ GoalTopic=goal
+Tool\ 2D\ Pose\ EstimateTopic=initialpose
+Camera\ Type=rviz::OrbitViewController
+Camera\ Config=1.16529 4.40033 3.27133 -0.15933 0.689146 -1.41357
+Property\ Grid\ State=selection=Fixed Frame;expanded=.Global Options,TF./BODY/BODY,TF./CHEST/CHEST,TF./LARM_LINK1/LARM_LINK1,TF./LARM_LINK2/LARM_LINK2,TF./LARM_LINK3/LARM_LINK3,TF./LARM_LINK4/LARM_LINK4,TF./LARM_LINK5/LARM_LINK5,TF./LARM_LINK6/LARM_LINK6,TF./LARM_LINK7/LARM_LINK7,TF./LLEG_LINK1/LLEG_LINK1,TF./LLEG_LINK2/LLEG_LINK2,TF./LLEG_LINK3/LLEG_LINK3,TF./LLEG_LINK4/LLEG_LINK4,TF./LLEG_LINK5/LLEG_LINK5,TF./LLEG_LINK6/LLEG_LINK6,TF./RARM_LINK1/RARM_LINK1,TF./RARM_LINK2/RARM_LINK2,TF./RARM_LINK3/RARM_LINK3,TF./RARM_LINK4/RARM_LINK4,TF./RARM_LINK5/RARM_LINK5,TF./RARM_LINK6/RARM_LINK6,TF./RARM_LINK7/RARM_LINK7,TF./RLEG_LINK1/RLEG_LINK1,TF./RLEG_LINK2/RLEG_LINK2,TF./RLEG_LINK3/RLEG_LINK3,TF./RLEG_LINK4/RLEG_LINK4,TF./RLEG_LINK5/RLEG_LINK5,TF./RLEG_LINK6/RLEG_LINK6,TF./VISION_SENSOR1/VISION_SENSOR1,TF./VISION_SENSOR2/VISION_SENSOR2,TF./WAIST/WAIST,TF./WAIST_LINK0/WAIST_LINK0,TF./WAIST_LINK1/WAIST_LINK1,TF./WAIST_LINK2/WAIST_LINK2,TF./WAIST_LINK3/WAIST_LINK3,TF./camera/camera,TF./odom/odom,TF./gsensor/gsensor,TF./gyrometer/gyrometer,TF./lfsensor/lfsensor,TF./lhsensor/lhsensor,TF./rfsensor/rfsensor,TF./rhsensor/rhsensor,TF.Enabled.TF.Tree,TF./odomTree/odom,TF./BODYTree/BODY,TF./WAISTTree/WAIST,TF./WAIST_LINK0Tree/WAIST_LINK0,TF./WAIST_LINK1Tree/WAIST_LINK1,TF./WAIST_LINK2Tree/WAIST_LINK2,TF./WAIST_LINK3Tree/WAIST_LINK3,TF./CHESTTree/CHEST,TF./LARM_LINK1Tree/LARM_LINK1,TF./LARM_LINK2Tree/LARM_LINK2,TF./LARM_LINK3Tree/LARM_LINK3,TF./LARM_LINK4Tree/LARM_LINK4,TF./LARM_LINK5Tree/LARM_LINK5,TF./LARM_LINK6Tree/LARM_LINK6,TF./LARM_LINK7Tree/LARM_LINK7,TF./lhsensorTree/lhsensor,TF./RARM_LINK1Tree/RARM_LINK1,TF./RARM_LINK2Tree/RARM_LINK2,TF./RARM_LINK3Tree/RARM_LINK3,TF./RARM_LINK4Tree/RARM_LINK4,TF./RARM_LINK5Tree/RARM_LINK5,TF./RARM_LINK6Tree/RARM_LINK6,TF./RARM_LINK7Tree/RARM_LINK7,TF./rhsensorTree/rhsensor,TF./VISION_SENSOR1Tree/VISION_SENSOR1,TF./VISION_SENSOR2Tree/VISION_SENSOR2,TF./cameraTree/camera,TF./LLEG_LINK1Tree/LLEG_LINK1,TF./LLEG_LINK2Tree/LLEG_LINK2,TF./LLEG_LINK3Tree/LLEG_LINK3,TF./LLEG_LINK4Tree/LLEG_LINK4,TF./LLEG_LINK5Tree/LLEG_LINK5,TF./LLEG_LINK6Tree/LLEG_LINK6,TF./lfsensorTree/lfsensor,TF./RLEG_LINK1Tree/RLEG_LINK1,TF./RLEG_LINK2Tree/RLEG_LINK2,TF./RLEG_LINK3Tree/RLEG_LINK3,TF./RLEG_LINK4Tree/RLEG_LINK4,TF./RLEG_LINK5Tree/RLEG_LINK5,TF./RLEG_LINK6Tree/RLEG_LINK6,TF./rfsensorTree/rfsensor,TF./gsensorTree/gsensor,TF./gyrometerTree/gyrometer,Camera.Enabled,Camera.Enabled.Camera.StatusTopStatus;scrollpos=0,0;splitterpos=150,288;ispageselected=1
+[Display0]
+Name=Grid
+Package=rviz
+ClassName=rviz::GridDisplay
+[Display1]
+Name=Robot Model
+Package=rviz
+ClassName=rviz::RobotModelDisplay
+[Display2]
+Name=TF
+Package=rviz
+ClassName=rviz::TFDisplay
+[Display3]
+Name=Camera
+Package=rviz
+ClassName=rviz::CameraDisplay
+[TF.]
+BODYEnabled=1
+CHESTEnabled=1
+LARM_LINK1Enabled=1
+LARM_LINK2Enabled=1
+LARM_LINK3Enabled=1
+LARM_LINK4Enabled=1
+LARM_LINK5Enabled=1
+LARM_LINK6Enabled=1
+LARM_LINK7Enabled=1
+LLEG_LINK1Enabled=1
+LLEG_LINK2Enabled=1
+LLEG_LINK3Enabled=1
+LLEG_LINK4Enabled=1
+LLEG_LINK5Enabled=1
+LLEG_LINK6Enabled=1
+RARM_LINK1Enabled=1
+RARM_LINK2Enabled=1
+RARM_LINK3Enabled=1
+RARM_LINK4Enabled=1
+RARM_LINK5Enabled=1
+RARM_LINK6Enabled=1
+RARM_LINK7Enabled=1
+RLEG_LINK1Enabled=1
+RLEG_LINK2Enabled=1
+RLEG_LINK3Enabled=1
+RLEG_LINK4Enabled=1
+RLEG_LINK5Enabled=1
+RLEG_LINK6Enabled=1
+VISION_SENSOR1Enabled=1
+VISION_SENSOR2Enabled=1
+WAISTEnabled=1
+WAIST_LINK0Enabled=1
+WAIST_LINK1Enabled=1
+WAIST_LINK2Enabled=1
+WAIST_LINK3Enabled=1
+cameraEnabled=1
+gsensorEnabled=1
+gyrometerEnabled=1
+lfsensorEnabled=1
+lhsensorEnabled=1
+odomEnabled=1
+rfsensorEnabled=1
+rhsensorEnabled=1

hrpsys_ros_bridge/launch/samplerobot_ros_bridge-grxui.launch

@@ -0,0 +1,18 @@
+<launch>
+
+  <include file="$(find hrpsys_ros_bridge)/launch/hrpsys_ros_bridge.launch">
+    <arg name="SIMULATOR_NAME" value="SampleRobot(Robot)0" />
+    <arg name="MODEL_FILE" value="$(find openhrp3)/share/OpenHRP-3.1/sample/model/sample1.wrl" />
+    <arg name="COLLADA_FILE" value="$(find hrpsys_ros_bridge_tutorials)/models/SampleRobot.dae" />
+  </include>
+
+  <node name="samplerobot_rviz" pkg="rviz" type="rviz" respawn="true"
+	args="-sync -d $(find hrpsys_ros_bridge_tutorials)/launch/samplerobot.vcg"
+        />
+
+  <node name="image_proc" pkg="image_proc" type="image_proc" output="screen" />
+  <include file="$(find roseus_tutorials)/launch/image-view.launch" />
+  <include file="$(find roseus_tutorials)/launch/camshiftdemo.launch" />
+
+</launch>
+

hrpsys_ros_bridge/launch/samplerobot_startup-grxui.launch

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="KILL_SERVERS" default="false" />
+  <include file="$(find hrpsys)/launch/hrpsys-grxui.launch">
+    <arg name="MODEL_FILE" value="$(find hrpsys)/scripts/SampleRobot_inHouse.xml" />
+    <arg name="KILL_SERVERS" default="$(arg KILL_SERVERS)" />
+  </include>
+</launch>

hrpsys_ros_bridge/models/samplerobot.yaml

@@ -0,0 +1,101 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rleg:
+  - RLEG_HIP_R      : rleg-crotch-r
+  - RLEG_HIP_P      : rleg-crotch-p
+  - RLEG_HIP_Y      : rleg-crotch-y
+  - RLEG_KNEE       : rleg-knee-p
+  - RLEG_ANKLE_P    : rleg-ankle-p
+  - RLEG_ANKLE_R    : rleg-ankle-r
+rarm:
+  - RARM_SHOULDER_P : rarm-shoulder-p
+  - RARM_SHOULDER_R : rarm-shoulder-r
+  - RARM_SHOULDER_Y : rarm-shoulder-y
+  - RARM_ELBOW      : rarm-elbow-p
+  - RARM_WRIST_Y    : rarm-wrist-y
+  - RARM_WRIST_P    : rarm-wrist-p
+  - RARM_WRIST_R    : rarm-wrist-r
+lleg:
+  - LLEG_HIP_R      : lleg-crotch-r
+  - LLEG_HIP_P      : lleg-crotch-p
+  - LLEG_HIP_Y      : lleg-crotch-y
+  - LLEG_KNEE       : lleg-knee-p
+  - LLEG_ANKLE_P    : lleg-ankle-p
+  - LLEG_ANKLE_R    : lleg-ankle-r
+larm:
+  - LARM_SHOULDER_P : larm-shoulder-p
+  - LARM_SHOULDER_R : larm-shoulder-r
+  - LARM_SHOULDER_Y : larm-shoulder-y
+  - LARM_ELBOW      : larm-elbow-p
+  - LARM_WRIST_Y    : larm-wrist-y
+  - LARM_WRIST_P    : larm-wrist-p
+  - LARM_WRIST_R    : larm-wrist-r
+torso:
+  - WAIST_P      : torso-waist-p
+  - WAIST_R      : torso-waist-r
+  - CHEST        : torso-waist-y
+
+##
+## end-coords
+##
+larm-end-coords:
+  translate : [0, 0, -0.12]
+  rotate    : [0, 1,  0, 90]
+  parent    : LARM_LINK6
+rarm-end-coords:
+  translate : [0, 0, -0.12]
+  rotate    : [0, 1,  0, 90]
+  parent    : RARM_LINK6
+lleg-end-coords:
+  translate : [0.00, 0, -0.07]
+rleg-end-coords:
+  translate : [0.00, 0, -0.07]
+head-end-coords:
+  translate : [0.08, 0, 0.13]
+  rotate    : [0, 1, 0, 90]
+
+##
+## reset-pose
+##
+angle-vector:
+# old reset-pose
+#  reset-pose : [0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # rleg
+#                30.0, 0.0, 0.0, -60.0,  9.0, -6.5,  36.5, # rarm
+#                0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # lleg
+#                30.0, 0.0, 0.0, -60.0, -9.0, -6.5, -36.5, # larm
+#                0.0, 0.0, 0.0] # torso
+  # openhrp reset pose from the initial line of OpenHRP-3.1/sample/controller/SampleController/etc/Sample.pos
+  reset-pose : [-0.004457, -21.6929, -0.01202, 47.6723, -25.93, 0.014025, # rleg
+                17.8356, -9.13759, -6.61188, -36.456, 0.0, 0.0, 0.0, # rarm
+                -0.004457, -21.6929, -0.01202, 47.6723, -25.93, 0.014025, # lleg
+                17.8356, 9.13759, 6.61188, -36.456, 0.0, 0.0, 0.0, # larm
+                0.0, 0.0, 0.0] # torso
+  reset-manip-pose : [0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # rleg
+                     30.0, 0.0, 0.0, -100.0,  9.0, -6.5,  36.5, # rarm
+                     0.0, -20.0, 0.0, 47.0, -27.0, 0.0, # lleg
+                     30.0, 0.0, 0.0, -100.0, -9.0, -6.5, -36.5, # larm
+                     0.0, 0.0, 0.0] # torso
+
+
+# for gazebo simulation
+replace_xmls:
+  - match_rule:
+      tag: gazebo
+      attribute_name: reference
+      attribute_value: LLEG_LINK6
+    replaced_xml: '<gazebo reference="LLEG_LINK6">\n    <kp>1000000.0</kp>\n    <kd>100.0</kd>\n    <mu1>1.5</mu1>\n    <mu2>1.5</mu2>\n    <fdir1>1 0 0</fdir1>\n    <maxVel>10.0</maxVel>\n    <minDepth>0.00</minDepth>\n  </gazebo>'
+  - match_rule:
+      tag: gazebo
+      attribute_name: reference
+      attribute_value: RLEG_LINK6
+    replaced_xml: '<gazebo reference="RLEG_LINK6">\n    <kp>1000000.0</kp>\n    <kd>100.0</kd>\n    <mu1>1.5</mu1>\n    <mu2>1.5</mu2>\n    <fdir1>1 0 0</fdir1>\n    <maxVel>10.0</maxVel>\n    <minDepth>0.00</minDepth>\n  </gazebo>'
+  - match_rule:
+      attribute_name: effort
+      attribute_value: 100
+    replaced_attribute_value: 200
+  - match_rule:
+      attribute_name: velocity
+      attribute_value: 0.5
+    replaced_attribute_value: 6.0