Ejemplo n.º 1
0
    /// <summary>
    ///     Publish the robot's current joint configuration, the target object's
    ///     position and rotation, and the target placement for the target object's
    ///     position and rotation.
    ///
    ///     Includes conversion from Unity coordinates to ROS coordinates, Forward Left Up
    /// </summary>
    public void PublishJoints()
    {
        MoverServiceRequest request = new MoverServiceRequest
        {
            joints_input =
            {
                joint_00 = jointArticulationBodies[0].xDrive.target,
                joint_01 = jointArticulationBodies[1].xDrive.target,
                joint_02 = jointArticulationBodies[2].xDrive.target,
                joint_03 = jointArticulationBodies[3].xDrive.target,
                joint_04 = jointArticulationBodies[4].xDrive.target,
                joint_05 = jointArticulationBodies[5].xDrive.target
            },
            pick_pose = new Pose
            {
                position = (target.transform.position + PICK_POSE_OFFSET).To <FLU>(),
                // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
                orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
            },
            place_pose = new Pose
            {
                position    = (targetPlacement.transform.position + PICK_POSE_OFFSET).To <FLU>(),
                orientation = pickOrientation.To <FLU>()
            }
        };

        ros.Send(rosJointPublishTopicName, request);
    }
    /// <summary>
    ///     Publish the robot's current joint configuration, the m_Target object's
    ///     position and rotation, and the m_Target placement for the m_Target object's
    ///     position and rotation.
    ///     Includes conversion from Unity coordinates to ROS coordinates, Forward Left Up
    /// </summary>
    public void PublishJoints()
    {
        var request = new MoverServiceRequest
        {
            joints_input = new NiryoMoveitJointsMsg(),
            pick_pose    = new PoseMsg
            {
                position = (m_Target.transform.position + m_PickPoseOffset).To <FLU>(),

                // The hardcoded x/z angles assure that the gripper is always positioned above the m_Target cube before grasping.
                orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To <FLU>()
            },
            place_pose = new PoseMsg
            {
                position    = (m_TargetPlacement.transform.position + m_PickPoseOffset).To <FLU>(),
                orientation = m_PickOrientation.To <FLU>()
            }
        };

        for (var i = 0; i < k_NumRobotJoints; i++)
        {
            request.joints_input.joints[i] = m_JointArticulationBodies[i].jointPosition[0];
        }

        m_Ros.Publish(rosJointPublishTopicName, request);
    }
Ejemplo n.º 3
0
    /// <summary>
    ///     Create a new MoverServiceRequest with the current values of the robot's joint angles,
    ///     the target cube's current position and rotation, and the targetPlacement position and rotation.
    ///
    ///     Call the MoverService using the ROSConnection and if a trajectory is successfully planned,
    ///     execute the trajectories in a coroutine.
    /// </summary>
    public void PublishJoints()
    {
        MoverServiceRequest request = new MoverServiceRequest();

        request.joints_input = CurrentJointConfig();

        // Pick Pose
        request.pick_pose = new RosMessageTypes.Geometry.Pose
        {
            position = new Point(
                target.transform.position.z,
                -target.transform.position.x,
                // Add pick pose offset to position the gripper above target to avoid collisions
                target.transform.position.y + pickPoseOffset
                ),
            // Orientation is hardcoded for this example so the gripper is always directly above the target object
            orientation = pickOrientation
        };

        // Place Pose
        request.place_pose = new RosMessageTypes.Geometry.Pose
        {
            position = new Point(
                targetPlacement.transform.position.z,
                -targetPlacement.transform.position.x,
                // Use the same pick pose offset so the target cube can be seen dropping into position
                targetPlacement.transform.position.y + pickPoseOffset
                ),
            // Orientation is hardcoded for this example so the gripper is always directly above the target object
            orientation = pickOrientation
        };

        ros.SendServiceMessage <MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
    }
Ejemplo n.º 4
0
    public void PublishJoints(Vector3 targetPos, Quaternion targetRot)
    {
        MoverServiceRequest request = new MoverServiceRequest();

        request.joints_input = CurrentJointConfig();

        // Pick Pose
        request.pick_pose = new RosMessageTypes.Geometry.Pose
        {
            position    = (targetPos + pickPoseOffset).To <FLU>(),
            orientation = Quaternion.Euler(90, targetRot.eulerAngles.y, 0).To <FLU>()
        };

        // Place Pose
        request.place_pose = new RosMessageTypes.Geometry.Pose
        {
            position    = (goal.position + placePoseOffset).To <FLU>(),
            orientation = pickOrientation.To <FLU>()
        };

        ros.SendServiceMessage <MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
    }
    /// <summary>
    ///     Create a new MoverServiceRequest with the current values of the robot's joint angles,
    ///     the target cube's current position and rotation, and the targetPlacement position and rotation.
    ///
    ///     Call the MoverService using the ROSConnection and if a trajectory is successfully planned,
    ///     execute the trajectories in a coroutine.
    /// </summary>
    public void PublishJoints()
    {
        MoverServiceRequest request = new MoverServiceRequest();

        request.joints_input = CurrentJointConfig();

        // Pick Pose
        request.pick_pose = new RosMessageTypes.Geometry.Pose
        {
            position = (target.transform.position + pickPoseOffset).To <FLU>(),
            // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
            orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
        };

        // Place Pose
        request.place_pose = new RosMessageTypes.Geometry.Pose
        {
            position    = (targetPlacement.transform.position + pickPoseOffset).To <FLU>(),
            orientation = pickOrientation.To <FLU>()
        };

        ros.SendServiceMessage <MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
    }