UnityとROSで仮想myCobotを操作！

docker run -p 6080:80 -p 10000:10000 -p 5005:5005 -v /dev:/dev --shm-size=512m tiryoh/ros-desktop-vnc:noetic

sudo apt update 
sudo apt upgrade -y

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws

catkin build

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc

sudo apt install python3-pip
pip install pyserial
pip install pymycobot

# GitHubからROS-TCP-Endpointをクローン
cd ~/catkin_ws/src
git clone https://github.com/Unity-Technologies/ROS-TCP-Endpoint

# ビルド
catkin build
source ~/.bashrc

roslaunch ros_tcp_endpoint endpoint.launch

sudo apt install -y ros-noetic-moveit
sudo apt install -y ros-noetic-ros-control ros-noetic-ros-controllers
sudo apt install -y ros-noetic-jsk-rviz-plugins

cd ~/catkin_ws/src
git clone https://github.com/elephantrobotics/mycobot_ros

catkin build
source ~/.bashrc

roslaunch mycobot_280_moveit demo.launch

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using JointStateMsg = RosMessageTypes.Sensor.JointStateMsg;

public class myCobot280M5Controller : MonoBehaviour
{
    public ArticulationBody joint2;
    public ArticulationBody joint3;
    public ArticulationBody joint4;
    public ArticulationBody joint5;
    public ArticulationBody joint6;
    public ArticulationBody joint6Flange;

    private ArticulationBody[] joint;
    private ROSConnection ros;


    // Start is called before the first frame update
    void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.Subscribe<JointStateMsg>("/move_group/fake_controller_joint_states", Callback);

        joint = new ArticulationBody[6];
        joint[0] = joint2;
        joint[1] = joint3;
        joint[2] = joint4;
        joint[3] = joint5;
        joint[4] = joint6;
        joint[5] = joint6Flange;
    }

    void Callback(JointStateMsg msg)
    {
        for (int i = 0; i < 6; i++)
        {
            ArticulationDrive aDrive = joint[i].xDrive;
            aDrive.target = Mathf.Rad2Deg * (float)msg.position[i];
            joint[i].xDrive = aDrive;
        }
    }
}

roslaunch mycobot_280_moveit mycobot_moveit.launch

roslaunch ros_tcp_endpoint endpoint.launch

cd ~/catkin_ws/src
catkin_create_pkg mycobot_unity rospy moveit_msgs geometry_msgs message_generation message_runtime 

cd mycobot_unity
mkdir msg
echo -e "float64[6] joints\ngeometry_msgs/Pose goal_pose" > msg/myCobotMoveitJoints.msg

add_message_files(
  FILES
  myCobotMoveitJoints.msg
)

generate_messages(
  DEPENDENCIES
  geometry_msgs
  moveit_msgs
  std_msgs
)

#!/usr/bin/env python
# coding: UTF-8

import sys
import rospy
import geometry_msgs.msg
import moveit_commander
from sensor_msgs.msg import JointState
from moveit_msgs.msg import RobotState
from moveit_msgs.msg import RobotTrajectory
from mycobot_unity.msg import myCobotMoveitJoints

class myCobotMover:
    def __init__(self, subTopicName, pubTopicName):
        self.sub = rospy.Subscriber(subTopicName, myCobotMoveitJoints, self.callback)
        self.pub = rospy.Publisher(pubTopicName, RobotTrajectory, queue_size=1)
        self.move_group = moveit_commander.MoveGroupCommander('arm_group')
        self.joint_state = JointState()
        self.joint_state.name = ['joint2_to_joint1', 'joint3_to_joint2', 'joint4_to_joint3', 'joint5_to_joint4', 'joint6_to_joint5', 'joint6output_to_joint6']
        self.robot_state = RobotState()
        self.pose_goal = geometry_msgs.msg.Pose()

    def callback(self, msg):
        plan = self.plan_trajectory(self.move_group, msg.goal_pose, msg.joints)
        self.move_group.stop()
        self.move_group.clear_pose_targets()

        if type(plan) is tuple:
            print("success>>>")
        else:
            print("fail>>>")

        self.pub.publish(plan[1])

    def plan_trajectory(self, move_group, pose_target, start_joints):
        self.joint_state.position = start_joints
        self.robot_state.joint_state = self.joint_state
        self.move_group.set_start_state(self.robot_state)

        self.pose_goal.position = pose_target.position
        self.pose_goal.orientation = pose_target.orientation
        self.move_group.set_joint_value_target(self.pose_goal, True)

        return self.move_group.plan()

def main():
    rospy.init_node('mycobot_mover')
    node = myCobotMover("mycobot_joints", "moveit_response")
    rospy.spin()

if __name__ == "__main__":
    main()

<launch>
  <!-- Unityの時間を使用するためにrosparamとして/use_sim_timeをtrueにする -->
  <param name="/use_sim_time" value="true" />

  <!-- UnityのROS-TCP-Endpointを起動 -->
  <include file="/home/ubuntu/catkin_ws/src/ROS-TCP-Endpoint/launch/endpoint.launch"/>

  <!-- myCobotのURDF、SRDF、その他の設定ファイルをパラメータサーバにロードする-->
  <include file="/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_280/mycobot_280_moveit/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- シミュレーションで使用する軌道制御器 -->
  <!-- 実機がある場合は実機付属の制御器が行う作業なので偽のコントローラと呼ばれているらしい -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="false"/>
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
  </node>

  <!-- 公開された関節状態が与えられたら、ロボットリンクのtfを公開する -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
  <!-- MoveIt! のメインとなる実行ファイルを実行-->
  <include file="/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_280/mycobot_280_moveit/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- mover.pyを起動 -->
  <node name="mover" pkg="mycobot_unity" type="mover.py" args="--wait" output="screen" />
</launch>

docker cp コンテナID:/home/ubuntu/catkin_ws/src/mycobot_unity .

float64[6] joints
geometry_msgs/Pose goal_pose

using RosMessageTypes.Geometry;
using RosMessageTypes.MycobotUnity;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using Unity.Robotics.UrdfImporter;
using UnityEngine;

public class myCobotSourceDestinationPublisher : MonoBehaviour
{
    private static readonly string TopicName = "/mycobot_joints";
    private static readonly Quaternion PickOrientation = Quaternion.Euler(90, 90, 0);

    public UrdfJointRevolute[] jointArticulationBodies;
    public GameObject target;
    private ROSConnection m_Ros;
    private MyCobotMoveitJointsMsg jointMsg;

    void Start()
    {
        m_Ros = ROSConnection.GetOrCreateInstance();
        m_Ros.RegisterPublisher<MyCobotMoveitJointsMsg>(TopicName);
        jointMsg = new MyCobotMoveitJointsMsg();
    }

    public void Publish()
    {
        for (var i = 0; i < jointArticulationBodies.Length; i++)
        {
            jointMsg.joints[i] = jointArticulationBodies[i].GetPosition();
        }

        jointMsg.goal_pose = new PoseMsg
        {
            position = target.transform.position.To<FLU>(),
            orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To<FLU>()
        };

        m_Ros.Publish(TopicName, jointMsg);
    }
}

using System.Collections;
using RosMessageTypes.Moveit;
using Unity.Robotics.ROSTCPConnector;
using UnityEngine;

public class myCobotMover : MonoBehaviour
{
    private static readonly string TopicName = "/moveit_response";
    private static readonly Quaternion PickOrientation = Quaternion.Euler(90, 90, 0);

    public ArticulationBody[] jointArticulationBodies;
    private ROSConnection m_Ros;

    void Start()
    {
        m_Ros = ROSConnection.GetOrCreateInstance();

        m_Ros.Subscribe<RobotTrajectoryMsg>(TopicName, Callback);
    }

    void Callback(RobotTrajectoryMsg trajectory)
    {
        if (trajectory!= null && trajectory.joint_trajectory.points.Length > 0) 
        {
            print("success>>>");
            StartCoroutine(ExecuteTrajectories(trajectory));
        }
        else
        {
            print("failed>>>");
        }
    }

    IEnumerator ExecuteTrajectories(RobotTrajectoryMsg trajectory)
    {
        foreach (var t in trajectory.joint_trajectory.points)
        {
            float[] result = new float[6];
            for (var i = 0; i < t.positions.Length; i++)
            {
                result[i] = (float)t.positions[i] * Mathf.Rad2Deg;
            }
            for (var i = 0; i < jointArticulationBodies.Length; i++)
            {
                var joint1XDrive = jointArticulationBodies[i].xDrive;
                joint1XDrive.target = result[i];
                jointArticulationBodies[i].xDrive = joint1XDrive;
            }

            yield return new WaitForSeconds(0.1f);
        }

        yield return new WaitForSeconds(0.1f);
    }
}

float64[6] joints
geometry_msgs/Pose goal_pose

float64[6] joints
geometry_msgs/Pose end_effector_pose
geometry_msgs/Pose goal_pose

using RosMessageTypes.Geometry;
using RosMessageTypes.MycobotUnity;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.Core;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using Unity.Robotics.UrdfImporter;
using UnityEngine;

public class myCobotTargetEndEffectorPosePublisher : MonoBehaviour
{
    private static readonly string TopicName = "/mycobot_tracking";
    private static readonly Quaternion PickOrientation = Quaternion.Euler(0, 0, 0);

    public UrdfJointRevolute[] jointArticulationBodies;
    public GameObject target;
    public GameObject endEffector;

    [SerializeField]
    double m_PublishRateHz = 1f;

    double m_LastPublishTimeSeconds;

    double PublishPeriodSeconds => 1.0f / m_PublishRateHz;

    bool ShouldPublishMessage => Clock.NowTimeInSeconds > m_LastPublishTimeSeconds + PublishPeriodSeconds;

    private ROSConnection m_Ros;
    private MyCobotTrackingTargetMsg jointMsg;

    void Start()
    {
        m_Ros = ROSConnection.GetOrCreateInstance();
        m_Ros.RegisterPublisher<MyCobotTrackingTargetMsg>(TopicName);
        jointMsg = new MyCobotTrackingTargetMsg();

        m_LastPublishTimeSeconds = Clock.time + PublishPeriodSeconds;
    }

    public void Publish()
    {
        // 現在の間接角度を格納
        for (var i = 0; i < jointArticulationBodies.Length; i++)
        {
            jointMsg.joints[i] = jointArticulationBodies[i].GetPosition();
        }
        // 目標位置を格納
        jointMsg.goal_pose = new PoseMsg
        {
            position = target.transform.position.To<FLU>(),
            orientation = Quaternion.Euler(target.transform.eulerAngles.x, 
                                           target.transform.eulerAngles.y, 
                                           target.transform.eulerAngles.z).To<FLU>()
        };
        // エンドエフェクタの位置を格納
        jointMsg.end_effector_pose = new PoseMsg
        {
            position = endEffector.transform.position.To<FLU>(),
            orientation = Quaternion.Euler(endEffector.transform.eulerAngles.x, 
                                           endEffector.transform.eulerAngles.y, 
                                           endEffector.transform.eulerAngles.z).To<FLU>()
        };

        m_Ros.Publish(TopicName, jointMsg);
        m_LastPublishTimeSeconds = Clock.FrameStartTimeInSeconds;
    }

    //常にパブリッシュし続ける
    private void Update()
    {
        if (ShouldPublishMessage)
        {
            Publish();
        }
    }
}

#!/usr/bin/env python
# coding: UTF-8

import sys
import rospy
import geometry_msgs.msg
import moveit_commander
from sensor_msgs.msg import JointState
from moveit_msgs.msg import RobotState
from moveit_msgs.msg import RobotTrajectory
from mycobot_unity.msg import myCobotTrackingTarget

class myCobotTracker:
    def __init__(self, subTopicName, pubTopicName):
        self.sub = rospy.Subscriber(subTopicName, myCobotTrackingTarget, self.callback)
        self.pub = rospy.Publisher(pubTopicName, RobotTrajectory, queue_size=1)
        self.move_group = moveit_commander.MoveGroupCommander('arm_group')
        self.joint_state = JointState()
        self.joint_state.name = ['joint2_to_joint1', 'joint3_to_joint2', 'joint4_to_joint3', 'joint5_to_joint4', 'joint6_to_joint5', 'joint6output_to_joint6']
        self.robot_state = RobotState()
        self.pose_goal = geometry_msgs.msg.Pose()

    def callback(self, msg):
        start_joints = msg.joints

        # ターゲットとエンドエフェクタの誤差を計算
        delta = abs(msg.end_effector_pose.position.x - msg.goal_pose.position.x) \
              + abs(msg.end_effector_pose.position.y - msg.goal_pose.position.y) \
              + abs(msg.end_effector_pose.position.z - msg.goal_pose.position.z) \
              + abs(msg.end_effector_pose.orientation.x - msg.goal_pose.orientation.x) \
              + abs(msg.end_effector_pose.orientation.y - msg.goal_pose.orientation.y) \
              + abs(msg.end_effector_pose.orientation.z - msg.goal_pose.orientation.z) \
              + abs(msg.end_effector_pose.orientation.w - msg.goal_pose.orientation.w)

        # 絶対誤差の累積が0.2よりも大きかった場合はプランニングを実施しロボットを動作させる
        if delta > 0.02:
            plan = self.plan_trajectory(self.move_group, msg.goal_pose, msg.joints)
            self.move_group.stop()
            self.move_group.clear_pose_targets()

            if plan[0]:
                print("success>>>")
                # プランニングが成功した場合はパブリッシュ
                self.pub.publish(plan[1])
            else:
                print("fail>>>")

    def plan_trajectory(self, move_group, pose_target, start_joints):
        self.joint_state.position = start_joints
        self.robot_state.joint_state = self.joint_state
        self.move_group.set_start_state(self.robot_state)

        self.pose_goal.position = pose_target.position
        self.pose_goal.orientation = pose_target.orientation
        self.move_group.set_joint_value_target(self.pose_goal, True)

        return self.move_group.plan()

def main():
    rospy.init_node('mycobot_tracker')
    node = myCobotTracker("mycobot_tracking", "moveit_response")
    rospy.spin()

if __name__ == "__main__":
    main()

import rospy
import moveit_commander
from math import pi
import tf
from geometry_msgs.msg import Quaternion, Vector3, Pose

# moveit_commander.roscpp_initialize()

rospy.init_node("mycobot_unity")
move_group = moveit_commander.MoveGroupCommander("arm_group")

pose_goal = Pose()
pose_goal.position = Vector3(0.1, 0.0, 0.1)
q = tf.transformations.quaternion_from_euler(0, 0, 0)
pose_goal.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
move_group.set_pose_target(pose_goal)
move_group.go(wait=True)

# pose_goal.position = Vector3(1e-6, 1e-6, 1e-6)
# q = tf.transformations.quaternion_from_euler(1e-6, 1e-6, 1e-6)

def move(x, y, z, a,b,c,d):
    pose_goal = Pose()
    pose_goal.position = Vector3(x, y, z)
    pose_goal.orientation = Quaternion(a, b, c, d)
    move_group.set_pose_target(pose_goal)
    move_group.go(wait=True)

<launch>
  <group ns="left">
    <include file="$(find mycobot_280_moveit)/launch/demo.launch"/>
  </group>
  <group ns="right">
    <include file="$(find mycobot_280_moveit)/launch/demo.launch"/>
  </group>
</launch>