tms_rp_rps_cnoid

Choreonoidとの連携部分(cnoid_planner_interface)について

cnoid_planner_interfaceのコンパイルを行う． choreooidのツールバーに「=Planner_ROS_InterfaceBar=」が追加されていれば導入完了．

=Planner_ROS_InterfaceBar=　使用方法

choreonoidを起動後，Robot，Object，Env，Wagon，Human，をあらかじめ設定しておくこと．

UpdatePosボタン：Robot，Wagon，Humanのデータベースの位置情報をchoreonoid上に反映． Start_RPS_Serverボタン：各srvの入力待機状態となる． (解除はchoreonoidを起動した端末上で「Ctrl＋C」)

利用可能な各種srv

具体的な使い方はrps_give_obj_commanderの該当箇所を参考にしてください．

• 物品把持計画

1. 通信路名："rps_cnoid_grasp_obj_planning"
2. 対応srv：tms_rp_msgs::rps_cnoid_grasp_obj_planning
3. 入力

• robot_pos：ロボットの位置
• pre_robot_joint_angle：ロボットの初期姿勢

4. 出力

• robot_joint_angle：物品把持動作の姿勢変化

6. 概要

• 入力されたロボット位置で，物品把持を行う． 物品位置はChoreonoid上でのobject位置とする． 出力姿勢の遷移順は以下のとおり． 入力した初期姿勢→物品把持前姿勢(手先位置を物品から少し手前とした位置)→物品把持姿勢→物品持ち上げ姿勢

• 物品受け渡し計画

1. 通信路名："rps_cnoid_give_obj_planning"
2. 対応srv：tms_rp_msgs::rps_cnoid_grasp_obj_planning
3. 入力

• robot_pos：ロボットの位置
• pre_robot_joint_angle：ロボットの初期姿勢
• obj_pos：受け渡し時の物品位置

4. 出力

• robot_joint_angle：物品把持動作の姿勢変化

5. 概要

• 入力されたロボット位置，受け渡し時の物品位置に対して物品把持姿勢計画を行う． 出力姿勢の遷移順は以下のとおり． 入力した初期姿勢→物品把持姿勢(物品受け渡し姿勢)

• ワゴン把持計画(把持姿勢のみ)

1. 通信路名："rps_cnoid_calc_grasp_wagon_pose"
2. 対応srv：tms_rp_msgs::rps_cnoid_grasp_wagon_planning
3. 入力

• robot_pos：ロボットの位置
• pre_robot_joint_angle：ロボットの初期姿勢
• wagon_pos：ワゴン位置

4. 出力

• robot_joint_angle：ワゴン把持姿勢

5. 概要

• 入力されたロボット位置，ワゴン位置に対してワゴン把持姿勢計画を行う． 出力はワゴン把持姿勢のみ．

• ワゴン把持計画

1. 通信路名："rps_cnoid_grasp_wagon_planning"
2. 対応srv：tms_rp_msgs::rps_cnoid_grasp_wagon_planning
3. 入力

• robot_pos：ロボットの位置
• pre_robot_joint_angle：ロボットの初期姿勢
• wagon_pos：ワゴン位置

4. 出力

• robot_joint_angle：初期姿勢からワゴン把持姿勢までの姿勢遷移

5. 概要

• 入力されたロボット位置，ワゴン位置に対してワゴン把持姿勢計画を行う． 出力姿勢の遷移順は以下のとおり． 入力した初期姿勢→ワゴン持前姿勢１(手先位置をワゴンから少し手前とした位置)→ワゴン持前姿勢２(手先位置をワゴン把持位置から少し左右に開いた位置) →ワゴン把持姿勢→ワゴン相対位置を理想位置(姿勢は除く)とした場合の把持姿勢→理想相対位置においてワゴン相対姿勢を0°とした場合の把持姿勢

• ワゴン手離し計画

1. 通信路名："rps_cnoid_release_wagon_planning"
2. 対応srv：tms_rp_msgs::rps_cnoid_grasp_wagon_planning
3. 入力

• robot_pos：ロボット位置
• wagon_pos：ワゴン位置

4. 出力

• robot_joint_angle：ワゴン把持姿勢からロボット基本姿勢までの遷移

5. 概要

• 入力されたロボット位置，ワゴン位置に対してワゴンから手を離す姿勢遷移計画を行う． 出力姿勢の遷移順は以下のとおり． ワゴン把持姿勢→ワゴン持前姿勢２(手先位置をワゴン把持位置から少し左右に開いた位置)→ワゴン持前姿勢１(手先位置をワゴンから少し手前とした位置) →ロボット基本姿勢

• 中間姿勢補間計画

1. 通信路名："rps_cnoid_PRM_planning"
2. 対応srv：tms_rp_msgs::rps_cnoid_PRM_planning
3. 入力

• in_motion_state：状態遷移

4. 出力

• out_motion_state：中間姿勢補間後の状態遷移

5. 補足:motion_stateについて　　

• position：各位置
• pushing_wagon_state：0=NOT_PUSHING　1=PUSHING
• grasping_object_state：0=NOT_GRASPING 1=UNDER_GRASPING 2=GRASPING 3=ON_WAGON

6. 概要

• 入力した状態遷移間の中間姿勢を，chorenoidのPRMプラグインを利用して補間する．