OGIMOノート~家族のためのモノづくり~

OGIMOテックノート ～家族のためのモノづくり～

重度障害の息子を持つ父親エンジニアの備忘録。自作の電子工作おもちゃ/リハビリ器具/ロボット関係の製作記録、思った事を残していきます

【アーム制御】CRANE+を使ったROSアーム制御①

アーム制御 ROS

はじめに

Turtlebot用ロボットアームを期間限定でレンタルできたので、勉強がてら遊んでみたいと思う。
うまく使えそうならば、ロボットアームを別途自作して、息子の食事介護支援などに応用できればよいなぁ、と企んでいます。

最初の目標 : グリッパー位置を任意の三次元位置に持っていく事。

使用ハードウェア

RT社製CRANE+
www.rt-shop.jp

アーム自由度4軸＋ハンド部１軸だそうです。サーボモータはDynamixel AX-12A。電源は12V。
USBシリアル通信で制御きるので、HostはROSが動く環境であれば何でもよさそう。

HW動作確認は、以下のサイトが詳しいのでこちらを見ながら進める。
個別のサーボモータを動かすことができればOK
www.rt-shop.jp

ソフトウェア環境構築

5つのサーボモータの角度をそれぞれアームの期待動作に応じて、個別計算&更新していく手段がまず考えられるが、
せっかくROSを使うのでROS既存パッケージを使った動作を試してみたいと思う。
そこで、マニュピュレータ制御用パッケージ moveit! を使ったアーム制御環境を構築する
参考にしたのは、以下のサイト
ROSを使用した画像処理とマニピュレータ制御 by gbiggs
このサイトに従って進めていく。
まずはインストール。

sudo apt-get install ros-kinetic-dynamixel-motor
sudo apt-get install ros-kinetic-moveit-*

上記で一旦インストールを完了させた後、CRANE+のROSパッケージをダウンロードしコンパイルする。

cd ~/catkin_ws/src/
git clone https://github.com/gbiggs/crane_plus_arm.git
cd ../
catkin_make

CRANE+のパッケージの詳細説明は引用サイトに記載されていますので、ここでは割愛。
ちなみに、私が途中でハマったのは、USB制御ポートが異なった場合の対応方法。
defaultでは、/dev/ttyUSB0 で制御する想定なのだが、Host PCのUSB構成次第ではここ以外を使わざるを得ない場合がある。
その場合は、crane_plus_arm/crane_plus_hardware/config/servo_controller_manager.yaml 内のport_name以下を変更すればよい。

moveitを使って任意位置にアームを移動

まず第一段階として、ユーザー操作で任意の位置にアームを動かす制御プログラムを作成する。
アーム位置を設定する手段として、PS4用コントローラを使用する。
PS4コントローラのアナログスティックを動かして、アーム位置をマニュアルで変更できる様にする。

制御ノートの追加

ワークスペース内に制御パッケージを追加する。

cd ~/catkin_ws/src/
catkin_create_pkg arm_control roscpp moveit_core moveit_ros_planning_interface moveit_visual_tools moveit_msgs moveit_commander tf actionlib control_msgs geometry_msgs shape_msgs trajectory_msgs

パッケージ内のCMakeLists.txtで以下を追加する

①find_packageに記載追加
find_package(Boost REQUIRED
  system
  filesystem
  date_time
  thread
)
②include_directoriesにBoostのディレクトリを追加。
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIR}
)
③コンパイル情報を追加
add_executable(ArmCtrlTrack src/arm_ctrl_track.cpp)
target_link_libraries(ArmCtrlTrack
 ${catkin_LIBRARIES}
 ${Boost_LIBRARIES}
)
制御プログラムの作成

C++で実際に実装する。ポイントとしては、
　・createTimer関数で定期的に実行関数をCallbackして、その関数内でarm座標をpublish(moveit関数を使用)
　・PS4コントローラ(joy)のSubscribe関数からアナログスティック情報を取得して座標情報(private変数)を都度更新
である。

アーム移動は、参照サイト同様に、geometry_msgs::PoseStamped型で座標定義してMoveIt!関数を使って移動先を指示&実行する。この時の座標系はbase_link、初期座標は arm.setNamedTarget("resting")に移動した後は、奥行きX = 0.18[m]、水平Y=0[m]、高さZ=0.10[m]からスタートする。この座標設定はCRANE＋が移動可能な場所を実験的に求めている。

グリッパー開閉は、参照サイト同様に、actionlibを利用し、control_msgs/GripperCommandActionアクションを使います。control_msgs/GripperCommandGoal.positionにグリッパー幅を指示する。初期値は0.10[m]でOpen状態とする。

#include <ros/ros.h>
#include <ros/wall_timer.h>
#include <math.h>
#include <sensor_msgs/Joy.h>
#include <actionlib/client/simple_action_client.h>
#include <moveit/move_group_interface/move_group.h>
#include <control_msgs/GripperCommandAction.h>
#include <geometry_msgs/PoseStamped.h>

class ArmCtrlTrack{
public:
	ArmCtrlTrack();
	~ArmCtrlTrack();
	void SetInitialPose();
	void timerCallback(const ros::TimerEvent&);
	void JoyConCallback(const sensor_msgs::Joy &joy_msg);
private:
	ros::Timer timer;
	ros::NodeHandle nh;
	ros::Subscriber joy_sub;
	geometry_msgs::PoseStamped arm_pose;
	geometry_msgs::Point pos_delta;			// 現在位置からのposition差分値
	control_msgs::GripperCommandGoal g_goal;
	float g_delta;

	moveit::planning_interface::MoveGroup arm_{"arm"};
	actionlib::SimpleActionClient<control_msgs::GripperCommandAction> gripper_{"/crane_plus_gripper/gripper_command"};
};

ArmCtrlTrack::ArmCtrlTrack(){
	timer   = nh.createTimer(ros::Duration(0.1), &ArmCtrlTrack::timerCallback, this);
	joy_sub = nh.subscribe("joy", 10, &ArmCtrlTrack::JoyConCallback, this);

	arm_pose.header.frame_id = "base_link";
	arm_pose.pose.position.x = 0.18;
	arm_pose.pose.position.y = 0.0;
	arm_pose.pose.position.z = 0.10;
	arm_pose.pose.orientation.x = 0.0;
	arm_pose.pose.orientation.y = 0.707106;
	arm_pose.pose.orientation.z = 0.0;
	arm_pose.pose.orientation.w = 0.707106;
	pos_delta.x = 0;
	pos_delta.y = 0;
	pos_delta.z = 0;
	g_delta = 0;
	gripper_.waitForServer();
	SetInitialPose();
}

ArmCtrlTrack::~ArmCtrlTrack(){
}

void ArmCtrlTrack::SetInitialPose(){
	// Planning interface for the arm
	arm_.setPoseReferenceFrame("base_link");
	arm_.setNamedTarget("resting");
	arm_.asyncMove();

	g_goal.command.position = 0.1;
	gripper_.sendGoal(g_goal);
}

void ArmCtrlTrack::JoyConCallback(const sensor_msgs::Joy &joy_msg){
	// 左スティック　垂直方向 : ARM y軸移動, 水平方向: ARM x軸方向、
	// 右スティック　垂直方向 : ARM Z軸移動, 水平方向, Griper Open/close
	pos_delta.x = joy_msg.axes[5] / 100 * ARM_UPDATE_TS * 2;
	pos_delta.y = joy_msg.axes[0] /100 * ARM_UPDATE_TS  *2;
	pos_delta.z = joy_msg.axes[1] / 100 * ARM_UPDATE_TS *2;
	g_delta     = joy_msg.axes[JOY_AXES_RIGHT_HORIZONTAL] / 100 * ARM_UPDATE_TS * 5;
}

void ArmCtrlTrack::timerCallback(const ros::TimerEvent&){
	// JOYCONからの操作情報をアップデート(ARM)
	if(pos_delta.x !=0 || pos_delta.y != 0 || pos_delta.z != 0){
		arm_pose.pose.position.x += pos_delta.x;
		arm_pose.pose.position.y += pos_delta.y;
		arm_pose.pose.position.z += pos_delta.z;
		ROS_WARN("[MOVEIT]Move Arm_position[%f, %f, %f]", arm_pose.pose.position.x, arm_pose.pose.position.y, arm_pose.pose.position.z);

		// ARM更新
		arm_.setPoseReferenceFrame("base_link");
		arm_.setPoseTarget(arm_pose);
		arm_.setGoalTolerance(0.02);
		arm_.asyncMove();
	}

	// JOYCONからの操作情報をアップデート(GRIPPER)
	if(g_delta != 0){
		g_goal.command.position += g_delta;
		if(g_goal.command.position < 0)	g_goal.command.position = 0;

		ROS_WARN("[MOVEIT]Move Gripper_position[%f]", g_goal.command.position);
		gripper_.sendGoal(g_goal);
	}
}

int main(int argc, char** argv){
	ros::init(argc, argv, "ArmCtrlTrack");
	ArmCtrlTrack client;
	ros::spin();
	return 0;
}

実装していく中でハマったポイントは以下の二点。（参照サイトに記載されたサンプルコードとの差分）
①move実行関数を arm_.Move() で実装した場合、動作完了するまでMove()関数から抜けられずそのまま座標更新ができなかった。そこで、非同期処理のarm_.asyncMove()に置き換える事で座標更新ができる様になった
②最初はmoveit::planning_interface::MoveGroup arm_("_arm")を:timerCallback関数内で都度定義していたが、その処理時間が想定以上にかかっていてtimerCallback処理時間溢れが発生していた。そこで、Classのprivate変数にコンストラクタ処理を含めて用意する方法に変更した。 (C++でClass変数設定時にコンストラクタを記載する手順を知らなくて苦戦した。。{}で記載できるんですね）
③コンストラクタ処理でgripper_.waitForServer()は必要。これがないと、ActionServerが準備できていない状態でClient側からコマンドが投げられてしまいエラーとなってしまう。

動作させるためのLaunchファイルも作成。制御用パッケージ内にlaunchフォルダを作成して、その中に以下の記載を追加。

[arm_ctrl_tracking.launch]
<?xml version="1.0"?>
    <include file="$(find crane_plus_hardware)/launch/start_arm_standalone.launch" />
    <include file="$(find crane_plus_moveit_config)/launch/move_group.launch" />
    <node pkg="joy" name="joycon" type="joy_node"/>
    <node pkg="arm_control" name="ArmCtrlTrack" type="ArmCtrlTrack"  output="screen"/>
</launch>
動作確認

動画撮影中。

意図した動作イメージを実現する事ができ、当初の目標は達成できた気がする。
しかし、応答速度が非常に遅い事 & 移動できない座標位置が非常に多い。。。。どうやらMoveit!自体は6軸アーム前提のソフトウェアのため、CRANE+の様な4軸アームに対しては逆運動学の式が最適に解けないのでは？？
なので、次回はMoveit!を使用せず、CRANE+のハードウェア仕様に特化して逆運動学を自力で解くプログラム作成にチャレンジしてみようと思う。果たして、レンタル期間中に間に合うか！？