public void PlanCartesianPath(int _instanceID, float _sockelOffsetX, float _sockelOffsetY, float _sockelOffsetZ, bool _useSpecialBoxGeometry)
        {
            instanceID      = _instanceID;
            _sockelOffsetY /= 100;

            /*
             * MOVE OBSTACLES BASED ON SOCKEL POSITION OFFSET
             */
            MoveCollisionObjects(_sockelOffsetX, _sockelOffsetY, _sockelOffsetZ, _useSpecialBoxGeometry);

            /*
             * ADJUST POSITIONS ON PATH BASED ON SOCKEL POSITION OFFSET
             */
            posesOnPath = new Pose[]
            {
                new Pose(new Point(aboveBox.x - _sockelOffsetZ, aboveBox.y - _sockelOffsetX, aboveBox.z - _sockelOffsetY),
                         orientation),
                new Pose(new Point(inBox.x - _sockelOffsetZ, inBox.y - _sockelOffsetX, inBox.z - _sockelOffsetY),
                         orientation),
                new Pose(new Point(aboveBox.x - _sockelOffsetZ, aboveBox.y - _sockelOffsetX, aboveBox.z - _sockelOffsetY + 0.20),
                         orientation),
                new Pose(new Point(aboveInducter.x - _sockelOffsetZ, aboveInducter.y - _sockelOffsetX, aboveInducter.z - _sockelOffsetY),
                         orientation),
                new Pose(new Point(onInducter.x - _sockelOffsetZ, onInducter.y - _sockelOffsetX, onInducter.z - _sockelOffsetY),
                         orientation),
                new Pose(new Point(aboveInducter.x - _sockelOffsetZ, aboveInducter.y - _sockelOffsetX, aboveInducter.z - _sockelOffsetY),
                         orientation),
                new Pose(new Point(aboveBox.x - _sockelOffsetZ, aboveBox.y - _sockelOffsetX, aboveBox.z - _sockelOffsetY),
                         orientation),
            };

            /*
             * PLAN IN CARTESIAN SPACE
             */
            var getCartesianPathRequest = new GetCartesianPathRequest();

            getCartesianPathRequest.header.Update();
            getCartesianPathRequest.group_name = "panda_arm";

            var robotJointState = new JointState
            {
                header   = new Header(),
                name     = jointGroup.jointNames,
                position = initialPosition
            };

            getCartesianPathRequest.start_state.joint_state = robotJointState;

            getCartesianPathRequest.link_name        = "panda_link8";
            getCartesianPathRequest.waypoints        = posesOnPath;
            getCartesianPathRequest.avoid_collisions = true;
            getCartesianPathRequest.max_step         = 0.01;

            rosSocket.CallService <GetCartesianPathRequest, GetCartesianPathResponse>("/compute_cartesian_path",
                                                                                      ServiceCallHandler, getCartesianPathRequest);
            OnServiceReceived.WaitOne();
            OnServiceReceived.Reset();
        }
Beispiel #2
0
 /// <summary> Setter constructor. </summary>
 public GetCartesianPath(GetCartesianPathRequest request)
 {
     Request  = request;
     Response = new GetCartesianPathResponse();
 }
Beispiel #3
0
 /// <summary> Empty constructor. </summary>
 public GetCartesianPath()
 {
     Request  = new GetCartesianPathRequest();
     Response = new GetCartesianPathResponse();
 }