Care-O-botのシミュレーション(アーム編)

気が向いたのでCare-O-botのアームのほうも動かします。

インストール
$ rosdep install cob3_gazebo cob3_arm

$ rosmake cob3_gazebo cob3_arm

動かす

$ roscd cob3_gazebo/ros/launch

$ roslaunch cob3_arm.launch

えー！

アームだけっすか。ビビった。

やはりJointSplineTrajectoryControllerを使って動かすみたいです。

$ rosrun cob3_arm simple_trajectory.py

とするとぐにゃぐにゃと動きます。

cob3_arm/ros/src/simple_trajectory.py

に軌道がかかれているので、これを変更すれば自由にアームを動かせますね。

次にGUIを使って操作する方法もあるのでこれを紹介します。

インストール

$ rosdep install cob3_dashboard

$ rosmake cob3_dashboard

実行

$ rosrun cob3_dashboard knoeppkes.py

ボタンを押すと動きますがあまり実用的ではなさそうです。

自分で軌道作って登録する感じでしょうか？

今日はここまで。

アームと台車が連動して動くかと思っていたのでちょっと残念ですね。

がんばればすぐできると思いますが。。。

Care-O-botのシミュレーションを試す

ドイツのCare-O-botのソフトがROSベースで公開されましたね。
私はこのロボットかなり好きだったので、ちょっとうれしいような、くやしいような。

さっそくかるーく試してみます。

まずはソフトのインストールから。

http://www.care-o-bot-research.org/software/care-o-bot-in-use

Gitを使ってDLするみたいなんで、git使っていない人はインストールしましょう。


$ sudo apt-get install git git-core gitk

で、次にgithubというサイトを使ってコードを配布しているので、
ここのアカウントがない人は作ります。

以下のサイトにいくと、sign upって書いてあると思うので、そこからsign upしてください。

http://github.com/ipa320/care-o-bot

※参考
そのときに、sshのpublicキーを要求されるので、

$ ssh-keygen -t rsa

として、
$HOME/.ssh/id_rsa.pub
の中身をコピペします。

gitは使ったことないので、不安ですが、
$ cd ~/ros/pkgs
$ git clone http://github.com/ipa320/care-o-bot.git care-o-bot

してコードをゲットしました。

$ cd ~/ros/pkgs/care-o-bot
$ . makeconfig
をターミナルで必ずするようにと書いてあります。

そして

$ rosdep install cob3_gazebo teleop_base
$ rosmake cob3_gazebo teleop_base

しましょう。

$ roscd cob3_gazebo/ros/launch

$ roslaunch cob3_pr2base.launch

シミュレータが上がりました。

ちょっと上から降ってくるので、びっくりしましたw

アームなしバージョンなんですね。

そして、以下のようにするとキーボードで動かせます。

$ rosrun teleop_base teleop_base_keyboard

今回はこれだけにしておきます。

アームもやりたい！

ROSチュートリアル in ICRA2010

ICRA2010という学会でROSのチュートリアルがあるみたいですね。

http://www.ros.org/wiki/Events/ICRA2010Tutorial

2日間にわたりがっつりです。
内容はこのブログと似た感じになりますね。

OpenRAVEをインストールする

プランニングのライブラリOpenRAVE
すごいですよね。
写真見るだけでもわくわくしますね。
http://openrave.programmingvision.com/index.php/Main_Page

これがROSとつながります。
http://www.ros.org/wiki/openrave
http://openrave.programmingvision.com/index.php/Robotics_Manipulation_System

今回はこれをインストールしましょう。

まずはsubversionでソースをもらいましょう。

svn co https://cmu-ros-pkg.svn.sourceforge.net/svnroot/cmu-ros-pkg/trunk ~/ros/pkgs/cmu-ros-pkg 

~/.octavercに以下を追加


more off
[status,rosoctpath] = system(['rospack find rosoct']);
rosoctpath = strtrim(rosoctpath);
addpath(fullfile(rosoctpath, 'octave'));
addpath(fullfile(rosoct_findpackage ('openrave'),'share','openrave','octave'))

$ rosdep install openraveros orrosplanning

で必要な外部ソフトをインストール。結構ありますね。

$ rosmake openraveros orrosplanning robot_openrave_control

でmakeします。

そして、~/ros/setup.shを以下のように編集します。

環境によって設定が変わると思うので参考程度に。

cp ~/ros/setup.sh ~/ros/setup.sh.org

してから編集しましょう。

最悪ROSが動かなくなります。

export ROS_ROOT=$HOME/ros/ros

export PATH=$ROS_ROOT/bin:$PATH

export PYTHONPATH=$ROS_ROOT/core/roslib/src:$PYTHONPATH

if [ ! "$ROS_MASTER_URI" ] ; then export ROS_MASTER_URI=http://localhost:11311 ; fi

export ROS_PACKAGE_PATH=/home/ogutti/ros/pkgs

export OCTAVE_PATH=$OCTAVE_PATH:$ROS_PACKAGE_PATH/ros_experimental/rosoct/octave

export ROS_PARALLEL_JOBS=-j1

export PATH=$PATH:`rospack find openrave`/bin

export PYTHONPATH=$PYTHONPATH:`rospack find openrave`/share/openrave

export OPENRAVE_DATA=$OPENRAVE_DATA:`rospack find visibilitygrasping`:`rospack find openrave`/share/openrave

source $ROS_ROOT/tools/rosbash/rosbash

現在のシェルを閉じて、

環境変数を新しくするために新しいシェルを立ち上げてください。

では試しましょう。

http://openrave.programmingvision.com/index.php/ROS:ControllingRobots

rosrun robot_openrave_control simulationserver --robotfile robots/schunk-lwa3.robot.xml --jointname j0 --jointname j1 --jointname j2 --jointname j3 --jointname j4 --jointname j5 --jointname j6

もしくは

roslaunch `rospack find robot_openrave_control`/schunkarm_sim.ros.xml

を実行しましょう。

これでコントローラーがあがりました。

そして以下のようにするとなんかロボットがでます。

roslaunch `rospack find robot_openrave_control`/wam_sim.ros.xml

なんか、いろいろ動いています。

今回は中身には触れませんが、動いたのでいろいろ試したいと思います。

i-SOBOT in Gazeboの進捗

まだサイズとか軸方向とか適当ですけど、

ロボットっぽくなってきましたねぇ。

まだ、腕がふらふらします。


サーボは初期化時に、
      joint->SetParam(dParamVel, 0);
      joint->SetParam(dParamFMax, JOINT_MAX_FORCE);

として、サーボループで、

      hj->SetParam(dParamVel, effort_command);

という感じでサーボしています。

どうなんでしょう。

ほとんどもうgazebo勉強記録になってきました。

URDFをさらしておきます。

<robot xmlns:xacro="http://ros.org/wiki/xacro"

       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"

       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"

       name="simple_box">

  <!-- *************** JOINT Definition *****************-->

  <xacro:macro name="my_joint" params="name parent child *axis *origin">

    <joint name="${name}" type="revolute" >

      <limit lower="-3" upper="3" effort="10.0" velocity="2.0" />

      <parent link="${parent}" />

      <child link="${child}" />

      <xacro:insert_block name="axis" />

      <xacro:insert_block name="origin" />

    </joint>

  </xacro:macro>

  <!-- *************** LINK Definition *****************-->

  <xacro:macro name="my_link" params="name mass color z *box">

    <link name="${name}">

      <inertial>

   <mass value="${mass}" />

<!-- center of mass (com) is defined w.r.t. link local coordinate system  -->

   <origin xyz="0 0 ${z}" />

   <inertia  ixx="${0.001 * mass}" ixy="0.0"  ixz="0.0"  iyy="${0.001 * mass}"  iyz="0.0"  izz="${0.001 * mass}" />

      </inertial>

      <visual>

<!-- visual origin is defined w.r.t. link local coordinate system -->

   <origin xyz="0 0 ${z}" rpy="0 0 0" />

   <geometry name="${name}_visual_geom">

          <xacro:insert_block name="box" />

   </geometry>

      </visual>

      <collision>

<!-- collision origin is defined w.r.t. link local coordinate system  -->

   <origin xyz="0 0 ${z}" rpy="0 0 0 " />

   <geometry name="${name}_collision_geom">

          <xacro:insert_block name="box" />

   </geometry>

      </collision>

    </link>

    <gazebo reference="${name}">

      <material>Gazebo/${color}</material>

      <turnGravityOff>false</turnGravityOff>

    </gazebo>

  </xacro:macro>

  <!-- *************** LEG Definition *****************-->

  <xacro:macro name="my_leg" params="name y">

    <!-- joint definition -->

    <xacro:my_joint name="${name}_joint1" parent="torso_link1" child="${name}_link1">

      <axis xyz="1 0 0"/>

      <origin xyz="0 ${y * 0.05} 0.0" rpy="0 0 0" />

    </xacro:my_joint>

    <xacro:my_joint name="${name}_joint2" parent="${name}_link1" child="${name}_link2">

      <axis xyz="0 1 0"/>

      <origin xyz="0 0 -0.02" rpy="0 0 0" />

    </xacro:my_joint>

    <xacro:my_joint name="${name}_joint3" parent="${name}_link2" child="${name}_link3">

      <axis xyz="0 1 0"/>

      <origin xyz="0 0 -0.04" rpy="0 0 0" />

    </xacro:my_joint>

    <xacro:my_joint name="${name}_joint4" parent="${name}_link3" child="${name}_link4">

      <axis xyz="0 1 0"/>

      <origin xyz="0 0 -0.04" rpy="0 0 0" />

    </xacro:my_joint>

    <xacro:my_joint name="${name}_joint5" parent="${name}_link4" child="${name}_link5">

      <axis xyz="1 0 0"/>

      <origin xyz="0 0 -0.04" rpy="0 0 0" />

    </xacro:my_joint>

    

    <xacro:my_link name="${name}_link1" mass="0.1" z="0.0" color="Blue">

      <box size="0.07 0.03 0.04" />

   </xacro:my_link>

    <xacro:my_link name="${name}_link2" mass="0.1" z="-0.02" color="Red">

      <box size="0.07 0.06 0.04" />

    </xacro:my_link>

    <xacro:my_link name="${name}_link3" mass="0.1" z="-0.02" color="Green">

      <box size="0.07 0.06 0.04" />

    </xacro:my_link>

    <xacro:my_link name="${name}_link4" mass="0.2" z="-0.02" color="Blue">

      <box size="0.10 0.06 0.04" />

    </xacro:my_link>

    <xacro:my_link name="${name}_link5" mass="0.2" z="-0.01" color="White">

      <box size="0.2 0.08 0.02" />

    </xacro:my_link>

  </xacro:macro>

  

  <!-- *************** ARM Definition *****************-->

  <xacro:macro name="my_arm" params="name y">

    <!-- joint definition -->

    <xacro:my_joint name="${name}_joint1" parent="torso_link1" child="${name}_link1">

      <axis xyz="0 1 0"/>

      <origin xyz="0 ${y * 0.08} 0.15" rpy="0 0 0" />

    </xacro:my_joint>

    

    <xacro:my_joint name="${name}_joint2" parent="${name}_link1" child="${name}_link2">

      <axis xyz="1 0 0"/>

      <origin xyz="0 ${y * 0.03} 0.01" rpy="0 0 0" />

    </xacro:my_joint>

    <xacro:my_joint name="${name}_joint3" parent="${name}_link2" child="${name}_link3">

      <axis xyz="1 0 0"/>

      <origin xyz="0 0 -0.085" rpy="0 0 0" />

    </xacro:my_joint>

    <xacro:my_link name="${name}_link1" mass="0.1" z="0.0" color="Blue">

      <box size="0.05 0.030 0.02" />

   </xacro:my_link>

    <xacro:my_link name="${name}_link2" mass="0.1" z="-0.035" color="Red">

      <box size="0.05 0.03 0.07" />

    </xacro:my_link>

    <xacro:my_link name="${name}_link3" mass="0.1" z="-0.035" color="Green">

      <box size="0.05 0.03 0.10" />

    </xacro:my_link>

  </xacro:macro>

  <!-- *************** HEAD Definition *****************-->

  <xacro:macro name="my_head" params="name">

    <!-- joint definition -->

    <xacro:my_joint name="${name}_joint1" parent="torso_link1" child="${name}_link1">

      <axis xyz="0 0 1"/>

      <origin xyz="0 0 0.2" rpy="0 0 0" />

    </xacro:my_joint>

    <xacro:my_link name="${name}_link1" mass="0.1" z="0.0" color="Green">

      <box size="0.08 0.07 0.05" />

    </xacro:my_link>

  </xacro:macro>

  <!-- *************** TORSO Definition *****************-->

  <xacro:my_link name="torso_link1" mass="0.1" z="0.1" color="Blue">

    <box size="0.12 0.15 0.15" />

  </xacro:my_link>

  <xacro:my_leg name="my_rleg" y="1" />

  <xacro:my_leg name="my_lleg" y="-1" />

  <xacro:my_arm name="my_rarm" y="1" />

  <xacro:my_arm name="my_larm" y="-1" />

  <xacro:my_head name="my_head1" />

  <gazebo>

    <controller:gazebo_ros_test name="gazebo_ros_test" plugin="libgazebo_ros_test.so">

      <interface:audio name="gazebo_ros_test_dummy_interface" />

      <alwaysOn>true</alwaysOn>

      <updateRate>1.0</updateRate>

      <timeout>5</timeout>

    </controller:gazebo_ros_test>

  </gazebo>

</robot>

PR2の台車が動かない？

以前、PR2の台車を動かすチュートリアルを紹介しました。
記事：http://ros-robot.blogspot.com/2010/01/pr2_06.html
本家チュートリアル：http://www.ros.org/wiki/pr2/Tutorials/Using%20the%20robot%20base%20controllers%20to%20drive%20the%20robot

これが現在動きません。

この原因を探っていくと、
~/ros/pkgs/pr2_controllers/pr2_mechanism_controllers/src/
にある
pr2_base_controller.cppの、


  //  cmd_sub_deprecated_ = root_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Pr2BaseController::commandCallback, this);
  cmd_sub_ = node_.subscribe<geometry_msgs::Twist>("command", 1, &Pr2BaseController::commandCallback, this);

という記述があります。

以前は

cmd_vel

というトピックで動かされていたのですが、これがdeprecatedになり、

現在は

command

というトピックに変更になったようです。

また、ネームスペースも切られていて、

$ rostopic list

とすると

/base_controller/command

というのがあります。

試しに動かすために、

$ rostopic pub /base_controller/command geometry_msgs/Twist '[0, 0, 0]' '[0, 0, 0]' 

とすると、ロボットが動きました。

ということで、このチュートリアルをどう変更すればいいか分かりますか？

私は以下のようにしました。

#include <iostream>

#include <ros/ros.h>

#include <geometry_msgs/Twist.h>

class RobotDriver

{

private:

  //! The node handle we'll be using

  ros::NodeHandle nh_;

  //! We will be publishing to the "cmd_vel" topic to issue commands

  ros::Publisher cmd_vel_pub_;

public:

  //! ROS node initialization

  RobotDriver(ros::NodeHandle &nh)

  {

    nh_ = nh;

    //set up the publisher for the cmd_vel topic

    cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("command", 1);

  }

  //! Loop forever while sending drive commands based on keyboard input

  bool driveKeyboard()

  {

    std::cout << "Type a command and then press enter.  "

      "Use '+' to move forward, 'l' to turn left, "

      "'r' to turn right, '.' to exit.\n";

    //we will be sending commands of type "twist"

    geometry_msgs::Twist base_cmd;

    char cmd[50];

    while(nh_.ok()){

      std::cin.getline(cmd, 50);

      if(cmd[0]!='+' && cmd[0]!='l' && cmd[0]!='r' && cmd[0]!='.')

      {

        std::cout << "unknown command:" << cmd << "\n";

        continue;

      }

      base_cmd.linear.x = base_cmd.linear.y = base_cmd.angular.z = 0;   

      //move forward

      if(cmd[0]=='+'){

        base_cmd.linear.x = 0.25;

      } 

      //turn left (yaw) and drive forward at the same time

      else if(cmd[0]=='l'){

        base_cmd.angular.z = 0.75;

        base_cmd.linear.x = 0.25;

      } 

      //turn right (yaw) and drive forward at the same time

      else if(cmd[0]=='r'){

        base_cmd.angular.z = -0.75;

        base_cmd.linear.x = 0.25;

      } 

      //quit

      else if(cmd[0]=='.'){

        break;

      }

      

      //publish the assembled command

      cmd_vel_pub_.publish(base_cmd);

    }

    return true;

  }

};

int main(int argc, char** argv)

{

  //init the ROS node

  ros::init(argc, argv, "robot_driver");

  ros::NodeHandle nh = ros::NodeHandle("base_controller");

  RobotDriver driver(nh);

  driver.driveKeyboard();

}

赤いところが変更したところです。

ROSの開発にドキュメントがついていけてない感じですね。。。

オリジナルロボット進捗その１

全然進んでいないのですが、

今やっとgazeboでサーボがまともに動くようになりました。

二足ロボットがやっと立てました。

シミュレーションパラメータも二足用になっていないので、チューンしないといけないかもしれません。