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(); }
/// <summary> Setter constructor. </summary> public GetCartesianPath(GetCartesianPathRequest request) { Request = request; Response = new GetCartesianPathResponse(); }
/// <summary> Empty constructor. </summary> public GetCartesianPath() { Request = new GetCartesianPathRequest(); Response = new GetCartesianPathResponse(); }